perm filename INTERP.OLD[OLD,HE] blob
sn#501011 filedate 1980-04-24 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00033 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00004 00002 .SBTTL Interpreter Data structures
C00015 00003 INTINIT, NEWENV, MINTS, MRKENV, MRKHDR
C00022 00004 Interpreter itself: INTERP
C00026 00005 GETARG, GETENV, GETSCA, GETVEC, GETTRN
C00033 00006 Variable declaration: MVAR, KVAR
C00045 00007 Stack ops: GTVAL, CHNGE, PUSHV
C00049 00008 Flow-of-control: PROC, RETURN
C00059 00009 ABORT, GODDT, NOOP, FORCHK, FOREND, CASE, JUMP, JUMPC
C00066 00010 SPAWN, SPROUT, TERMINATE
C00074 00011 Booleans: SLE,SLT,SGE,SGT,SEQ,SNE,AND,LOR,NOT,LXOR,EQV
C00080 00012 return scalars: SABS,SADD,SSUB,SMUL,SDIV,SNEG,SEXP,MAX,MIN,INT,IDIV,MOD
C00086 00013 VDOT, VMAG, SSBRTN
C00091 00014 Vector utilities: UNITV, CROSV
C00097 00015 TRANS extraction routines: TPOS, TORIEN, TAXIS, TMAGN
C00105 00016 Return vectors: SVMUL, VSDIV, TVMUL, VMAKE, VADD, VSUB
C00112 00017 Return a trans: TMAKE, TVADD, TVSUB, TTMUL, TINVRT, VSAXWR, CONSTR
C00126 00018 Affixment: AFFIX
C00135 00019 UNFIX
C00141 00020 Motion: MOVE, CENTER, OPERATE, STOP
C00145 00021 Common code for motions: MOVSTA & OPMOV
C00154 00022 Error recovery for motions: RETRY, FINISH, PARK
C00160 00023 Force system routines: SETBAS, WRIST, STIFF, GATHR
C00165 00024 Motion auxilary functions: TABOFS, WHERE, NOTICE
C00169 00025 Condition monitors: CMMAK
C00178 00026 CMENBL, CMDSBL, CMDEST, CMTRIG, CMSKED, CMWAIT, CMUNCR
C00188 00027 CMDONE, CMDUR, CMFORCE, CMSENSE, COMPLY, CMPOFF, VMKFRC, TFRCST
C00197 00028 Events: SIGNAL, WAITE, PAUSE
C00201 00029 Input routines: PROMPT, QUERY, SCALRD
C00204 00030 Output routines: PRINT, VALPRN, VARPRN, TACKVAL, TYPVAL, CVFX
C00210 00031 BREAK, NOOP, TOPAL
C00212 00032 Initialization ops: PROG, ENDP, FIXIT
C00217 00033 BUGS
C00218 ENDMK
C⊗;
.SBTTL Interpreter ;Data structures
COMMENT ⊗
Register uses in the interpreter:
R5 used by some routines as the display register
R4 points to interpreter status block
R3 interpreter stack pointer
R2 not used by the main interpreter loop. Can be munged by
any primary interpreter routine.
Each interpreter has a stack which it uses to store pointers to
currently "open" variables. During the course of a calculation,
operands and temporary result cells will be open in this fashion.
The "interpreter stack" is pointed to by R3. When a new interpreter
is sprouted, it is given a new stack area. Each interpreter has
certain status information which facilitates transfer of control
between interpreters. This information is kept in the interpreter
status block, which is always pointed to by R4. Most important are
the IPC, the Interpreter Program Counter, the ENV, which points to
the local environment, and LEV, which stores the current lexical
level.
Each procedure has an environment, which is a data area holding
information vital to that procedure. This includes pointers to all
the variables local to that procedure, and return information.
⊗
INSTSZ == 20 ;Size of an interpreter stack
;Interpreter status block
II == 0
XX IPC ;Interpreter program counter. Leave this as first field!
XX ENV ;Location of local environment
XX LEV ;Lexical level of current execution
XX NXTINT ;Next interpreter in the list. For GC of the stacks & env
XX STKBAS ;Location of start of stack area. Needed
;for eventual reclamation.
XX PDB ;Location of process descriptor block (for reclamation)
XX EVT ;The event to signal as this interpreter goes away
XX CMCB ;Pointer to c-m control block if this is a checker or a body
ISBS == II/2 ;Size (in words) of interpreter status block
;Procedure descriptor fields
II == 0
XX IPC ;Pointer to code for procedure body
XX ENV ;Pointer to current environment when procedure is defined
XX LEV ;Lexical level of procedure
XX ENVSZ ;Size of environment needed by procedure
XX NARGS ;Number of arguments for procedure
; arg 1 - arg n ;argument list giving access & data types
PRODSZ == II/2 ;Size (in words) of procedure descriptor - minus arg list
;Procedure header fields
II == 0
XX NXTPRO ;Pointer to next procedure in chain (for marking purposes)
XX OIPC ;Old IPC. Program counter for calling process.
XX OENV ;Old environment, the one for the calling process.
XX OLEV ;Old level. The lexical level of calling process.
XX STKBAS ;Location of start of stack area. Needed
;for eventual reclamation.
XX OR3 ;Old R3 - we will restore it when we return
XX OR5 ;Old R5 of calling process - R5 will point to this header
XX OSTOP ;Old R3 stack top
XX OSBOT ;Old R3 stack bottom
PROHSZ == II/2 ;Size (in words) of procedure header
;Fixed fields in the environment of each process
II == 0
XX SLINK ;Pointer to environment of next (outer, lower
; numbered) block
XX LVARS ;First location where pointers to local variables go
ENVSIZ == II/2 ;Size (in words) of environment header
;Each environment entry consists of two words. The first gives the accessing
;method & the data type, while the second contains a pointer to the
;value/header.
; Data types
SCLTYP == 1
VECTYP == 2
TRNTYP == 3
EVNTYP == 4
CMNTYP == 5
; Access methods
; DIRECT == 0 ;Pointer to value
HDRTYP == 400 ;Pointer to frame header
ARYTYP == 1000 ;Pointer to array header
REFTYP == 2000 ;Indirect pointer to entry in another environment
PROTYP == 4000 ;Pointer to procedure descriptor
; Mechanism bits.
YARM == 1
YHAND == 2
BARM == 4
BHAND == 10
ANARM == YARM + BARM
AHAND == YHAND + BHAND
VISE == 20
DRIVER == 40
; Servo bits.
YARMSB == 176000
YHANDSB == 1000
BARMSB == 770
BHANDSB == 4
VISESB == 2
DRVRSB == 1
; Table offsets for various mechanisms.
OFYARM == 0
OFYHAND == 6*2
OFBARM == 7*2
OFBHAND == 16*2
OFVISE == 17*2
OFDRIVER == 20*2
; Environment offsets for the various mechanisms
YAOFST == 0
YHOFST == 1
BAOFST == 2
BHOFST == 3
; Environment offsets for the deproach variables
YDEPROACH == 4
BDEPROACH == 5
; Environment offsets for devices: vise & driver
; VISE == 6
; FIXEDJAW == 7
; MOVINGJAW == 10
; VISEOP == 11
; DRIVERGRASP == 12
; DRIVERTIP == 13
; DRIVERTRANS == 13
; TURNS == 15
; Environment offsets for devices: vise & driver
; barm-error == 16
; bhand-error == 17
; yarm-error == 20
; yhand-error == 21
; vise-error == 22
; driver-error == 23
; speed-factor == 24
DATA
SYSENV: 0 ;SLINK = nil
SYSEND ;LVARS
HDRTYP+TRNTYP, YARMHD
HDRTYP+SCLTYP, YHANDH
HDRTYP+TRNTYP, BARMHD
HDRTYP+SCLTYP, BHANDH
TRNTYP, NILTRN ;ydeproach
TRNTYP, NILTRN ;bdeproach
HDRTYP+SCLTYP, VISEH ;vise opening width
HDRTYP+TRNTYP, FJAWH ;vise fixed jaw
HDRTYP+TRNTYP, MJAWH ;vise moving jaw
VISOP: TRNTYP, 0 ;vise opening
HDRTYP+TRNTYP, DRVGRP ;driver grasp
HDRTYP+TRNTYP, DRVTIP ;driver tip
TRNTYP, 0 ;driver trans (initially undefined)
HDRTYP+SCLTYP, TURNSH ;driver turns
BAERR: SCLTYP, 0 ;barm-error
BHERR: SCLTYP, 0 ;bhand-error
YAERR: SCLTYP, 0 ;yarm-error
YHERR: SCLTYP, 0 ;yhand-error
VIERR: SCLTYP, 0 ;vise-error
DRERR: SCLTYP, 0 ;driver-error
SPDFAC: SCLTYP, 0 ;speed-factor
FINITR: TRNTYP, 0 ;trans for FINISH to use
FINIOF==<FINITR-<SYSENV+4>>/4 ;calculate offset
SYSEND: 0
YARMHD: 0 ;Header for YARM
0 ;type = trans device
YARMSB ;servo bits for coefficient list
0
0 ;calc list = nil
YARM ;mechanism bits
YHANDH: 0 ;Header for YHAND
SCDEV ;type = scalar device
YHANDSB ;servo bits for coefficient list
0
0 ;calc list = nil
YHAND ;mechanism bits
BARMHD: 0 ;Header for BARM
0 ;type = trans device
BARMSB ;servo bits for coefficient list
0
0 ;calc list = nil
BARM ;mechanism bits
BHANDH: 0 ;Header for BHAND
SCDEV ;type = scalar device
BHANDSB ;servo bits for coefficient list
0
0 ;calc list = nil
BHAND ;mechanism bits
VISEH: 0 ;Header for VISE
SCDEV ;type = scalar device
VISESB ;servo bits for coefficient list
0
0 ;calc list = nil
VISE ;mechanism bits
FJAWH: 0 ;Header for vise FIXED JAW
1 ;variable
1 ;not valid
0 ;no value
0 ;calc list = nil
MJAWH: 0 ;Header for vise MOVING JAW
1 ;variable
1 ;not valid
0 ;no value
0 ;calc list = nil
DRVGRP: 0 ;Header for driver grasp
1 ;variable
1 ;not valid
0 ;no value
0 ;calc list = nil
DRVTIP: 0 ;Header for driver tip
1 ;variable
1 ;not valid
0 ;no value
0 ;calc list = nil
TURNSH: 0 ;Header for DRIVER
SCDEV ;type = scalar device
DRVRSB ;servo bits for coefficient list
0
0 ;calc list = nil
DRIVER ;mechanism bits
;INTINIT, NEWENV, MINTS, MRKENV, MRKHDR
INTEVT: 0 ;The event that interlocks references to ISTBLK.
CODE
INTINIT: ;Initializes the above events
EVMAK ;Initialize the INTEVT.
MOV (SP),INTEVT;
EVSIG
RTS PC ;Done
MINTS: ;Marking method for interpeters
PUSH <R2,R3> ;Save R2 & R3
EVWAIT INTEVT ;Enter critical region
MOV NXTINT+ISTBLK,R2 ;R2 ← LOC[first real interpeter status block]
BEQ 4$ ;If none, then done
1$: JSR PC,MRKSTK ;Mark the stack
;mark the environment
3$: MOV ENV(R2),R3 ;R3 ← environment
JSR PC,MRKENV ;Go mark the environment
MOV NXTINT(R2),R2 ;R2 ← LOC[next interpreter status block]
BNE 1$ ;Repeat as necessary
;handle currently active procedures
4$: MOV PROLST,R2 ;R2 ← LOC[first procedure header]
BEQ 6$ ; if any
5$: JSR PC,MRKSTK ;Mark the procedure's stack
MOV OENV(R2),R3 ;Mark the environment of the caller
JSR PC,MRKENV
MOV NXTPRO(R2),R2 ;R2 ← LOC[next procedure header]
BNE 5$ ;Repeat as necessary
;mark the system environment
6$: MOV #SYSENV,R3
JSR PC,MRKENV ;Mark the system variables
POP <R3,R2> ;Restore R3 & R2
EVSIG INTEVT
RTS PC ;Return
MRKSTK: ;mark the stack pointed to by R2
MOV STKBAS(R2),R3 ;R3 ← LOC[interpreter stack base]
ADD #2*INSTSZ,R3 ;R3 ← LOC[verge of new stack] (INSTSZ is in bytes)
1$: MOV -(R3),R0 ;R0 ← stack entry
BEQ 2$ ;If 0, then end of stack (RF: this wont work!!)
JSR PC,MARKQ
MOV R0,(R3) ;Put it back (compacting may move it)
BR 1$
2$: RTS PC
MRKENV: PUSH <R2,LVARS(R3)> ;Save R2 & LOC[first free variable entry]
ADD #2*ENVSIZ,R3 ;R3 ← LOC[first variable entry]
1$: CMP R3,(SP) ;See if we're done
BHIS 20$
TSTB 1(R3) ;Check access method
BNE 3$
CMP (R3),#EVNTYP ;Check if event or cmon
BGE 2$ ; & if so don't mark it
MOV 2(R3),R0 ;Direct - R0 ← LOC[value]
JSR PC,MARKQ ;Mark it
MOV R0,2(R3) ;Compacting might move it
2$: CMP (R3)+,(R3)+ ;R3 ← LOC[next variable entry to mark]
BR 1$ ;Keep going
3$: BIT #REFTYP,(R3) ;Indirect reference?
BNE 2$ ; skip it if so
BIT #HDRTYP,(R3) ;Frame header?
BEQ 10$ ; skip ahead if not
MOV 2(R3),R2 ;R2 ← LOC[frame header]
JSR PC,MRKHDR ;Mark it
BR 2$
10$: BIT #ARYTYP,(R3) ;Do we have an array to mark?
BEQ 2$ ;Skip to next if not (don't mark procedure desc)
MOV 2(R3),R2 ;R2 ← LOC[array header]
MOV (R2)+,R1 ;R1 ← # of dimensions
MUL #6,R1 ;R1 ← # bytes taken up by bounds info
ADD R2,R1 ;R1 ← LOC[beginning of array environment]
MOV R1,R0
MOV (R2)+,R1 ;R1 ← upper bound of 1st subscript
SUB (R2)+,R1
INC R1 ;R1 ← range of 1st subscript
MUL (R2)+,R1 ;R1 ← size of array
MOV R0,R2 ;R2 ← LOC[first entry of array environment]
CMPB (R2),#EVNTYP ;Check datatype of array
BGE 2$ ;Don't bother marking an array of events
PUSH <R3,R4>
MOV R1,R4 ;R4 ← # of entries to mark
MOV R2,R3 ;R3 ← LOC[first entry to mark]
11$: BIT #HDRTYP,(R3)+ ;Do we have a frame header to mark?
BEQ 12$
MOV (R3)+,R2 ;R2← LOC[frame header]
JSR PC,MRKHDR ;Mark the frame & its affixments
BR 13$
12$: MOV (R3),R0 ;R0 ← LOC[value pointer]
JSR PC,MARKQ ;Mark it
MOV R0,(R3)+ ;Compacting might move it
13$: SOB R4,11$ ;Mark everyone
POP <R4,R3>
BR 2$ ;Done with array - go mark rest of environment
20$: TST (SP)+ ;Clean LVARS off stack
POP <R2> ;Restore R2
RTS PC
MRKHDR: ;R2 ← LOC[frame header to mark]
BIT #FTYPE,TYPE(R2) ;See if device
BEQ 1$ ;Don't mark value for devices
MOV VAL(R2),R0
JSR PC,MARKQ ;Mark it
MOV R0,VAL(R2)
1$: MOV CALCS(R2),R2 ;R2 ← list of affixments
BEQ 4$ ; if any
2$: BIT #FRAME2+EXPTRN,TYPE(R2) ;See if we should mark the trans
BNE 3$
MOV TRANS(R2),R0
JSR PC,MARKQ ;Mark it
MOV R0,TRANS(R2)
3$: MOV (R2),R2 ;Deal with the next affixment
BNE 2$
4$: RTS PC ;Done
NEWENV: ;Gets a new environment, returns address in R0.
MOV @(R4),R0 ;Get number of variables used in this environment
ADD #2,(R4) ;Bump IPC
ASL R0 ;Need 2 words/variable
ADD #ENVSIZ,R0 ;Add in header size
JSR PC,GTFREE ;Allocate from large blocks
MOV R0,LVARS(R0)
ADD #2*ENVSIZ,LVARS(R0) ;Initialize where the first free entry
RTS PC ; should go
;Interpreter itself: INTERP
.MACRO MAKEOP CNAME, ANAME ;Compiler name, Address name
XX CNAME
ANAME
.ENDM
DATA
;The interpreter operation table
INTOPS: MAKEOP XINVALID,INVALID ;Illegal instruction
.INSRT INTOPS.PAL[AL,HE]
INSEND = II ;Marks the end of the instructions
CODE
.MACRO FETCH foo
MOV @(R4),foo ;Get next interpreter instruction in foo
ADD #2,(R4) ;Bump IPC
.ENDM
.MACRO BMPIPC
ADD #2,(R4) ;Bump IPC
.ENDM
.MACRO BACKIPC
SUB #2,(R4) ;Backup IPC
.ENDM
.MACRO CCC ;Clear condition code
; CLR R0 ;Clear condition code. Not used right now.
.ENDM
.MACRO SCC ;Set condition code
; MOV #2,R0 ;Set condition code. Not used right now. (maybe use TST PC)
.ENDM
INTERP:
MOV R3,R0 ;Save limits of the interpreter stack for error checking.
SUB #2*INSTSZ,R0 ;R0 ← Stack base (hopefully)
PUSH <R0,R3> ;Stack bottom then top
INT1: CMP R3,2(SP) ;Interpreter stack overflow?
BHI 1$ ;No. Go to next instruction.
ALERR INTMS3 ;Yes. Complain.
1$: CMP R3,(SP) ;Interpreter stack underflow?
BLOS 2$ ;No. Go to next instruction.
ALERR INTMS4 ;Yes. Complain.
2$: CLR -2(R3) ;Zero above top of stack - to keep MINTS happy
MOV @IPC(R4),R0 ;R0 ← next instruction
BLE INVALID ;Instruction out of range
CMP R0,#INSEND ;Is instruction too large?
BLE INT2 ;No.
INVALID::ALERR INTMS1 ;Yes. complain.
INT2:: BMPIPC ;Bump IPC
JSR PC,@INTOPS(R0) ;Call the appropriate routine
BR INT1 ;Repeat interpreter loop
DATA
INTMS1::ASCIE /INTERPRETER INSTRUCTION OUT OF RANGE/
;INTMS2::ASCIE /INTERPRETED INSTRUCTION RETURNED FAILURE/
INTMS3::ASCIE /INTERPRETER STACK OVERFLOW/
INTMS4::ASCIE /INTERPRETER STACK UNDERFLOW/
CODE
; GETARG, GETENV, GETSCA, GETVEC, GETTRN
GETARG:
COMMENT ⊗ Called with R0=variable name (level-offset), returns with R0 pointing
to the environment entry for the variable, after performing any array
references. ⊗
JSR PC,GETENV ;Get the environment pointer in R0
BIT #ARYTYP,(R0) ;Do we have an array to access?
BEQ 10$
PUSH <R2>
MOV 2(R0),R2 ;R2 ← LOC[array header]
MOV (R2)+,R0 ;R0 ← # of dimensions of array
CLR -(SP) ;Set offset to zero
3$: LDF @(R3)+,AC0 ;Get value of subscript
STCFI AC0,R1 ;Convert it to integer & store it in R1
CMP R1,(R2)+ ;Check we don't exceed upper bound
BLOS 4$
PUSH <R0> ; no good
ALERR 20$ ;Complain
POP <R0>
MOV -2(R2),R1 ;Use upper bound as default
4$: SUB (R2)+,R1 ;Check we also satisfy lower bound
BHIS 5$
PUSH <R0> ; no good
ALERR 21$ ;Complain
POP <R0>
CLR R1 ;Use lower bound as default
5$: MUL (R2)+,R1 ;Multiply by MULT[i]
ADD R1,(SP) ;Update offset
SOB R0,3$ ;Do all the subscripts
POP <R0> ;R0 ← offset into array
ASH #2,R0 ;Convert to byte offset into environment
ADD R2,R0 ;Add base address to offset
POP <R2>
10$: RTS PC ;Return with R0 = LOC[Env entry for variable]
DATA
20$: .ASCIZ /Subscript index greater than upper bound./
21$: ASCIE </Subscript index less than lower bound./>
CODE
GETENV: ;Auxiliary routine - called by GETARG & PROC (& WRIST temp)
COMMENT ⊗
Arguments:
R0=variable name: high byte is lexical level, low byte is offset.
R4=pointer to interpreter status block.
Result:
R0← pointer to address of desired variable.
R1 clobbered.
This routine returns in R0 a pointer to the location in the current
environment (or, if necessary, more global environment) which
points to the variable which is named in R0. Any indirect references
are resolved. ⊗
PUSH <R2> ;Save R2
MOV R0,R1
BIC #177400,R1 ;R1 ← Offset desired
CLRB R0
SWAB R0 ;R0 ← Lexical level
BNE 1$
MOV #SYSENV,R2 ;For lexical level 0 use system environment
BR 3$
1$: MOV ENV(R4),R2 ;R2 ← LOC[local environment]
SUB LEV(R4),R0 ;R0 ← Difference in levels: desired-got
BEQ 3$ ;Diff=0; can use R2 as pointer at right base.
BHI 6$ ;If diff>0, then value inaccessible.
NEG R0 ;Make diff>0 so we can use a SOB instruction
2$: MOV SLINK(R2),R2;Must go up a level. R2 ← LOC[more global environment]
SOB R0,2$ ;If not yet good, then move up another level
3$: ASH #2,R1 ;Convert offset to environment pointer (each entry = 2 wds)
ADD #2*ENVSIZ,R1 ;Add in environment header
ADD R2,R1 ;R1 ← environment + offset = location of desired entry
MOV R1,R0
POP <R2> ;Restore R2.
4$: BIT #REFTYP,(R0) ;An indirect reference?
BEQ 5$
MOV 2(R0),R0 ; yes - fetch actual reference
BR 4$ ;Check for multiple levels of indirection
5$: RTS PC ;Done.
6$: PUNT 7$
DATA
7$: ASCIE /ATTEMPT TO ACCESS UNAVAILABLE VARIABLE/
CODE
GETSCA: ;Gets place for a scalar result, and places a pointer on
;the interpreter stack. Location is returned in R0.
;Simple procedure.
MOV #SCASPC,R0
JSR PC,GETSBK ;Allocate from small blocks
MOV R0,-(R3) ;Push new value cell pointer on interpreter stack.
EVSIG SBEVT ;End of critical section
RTS PC ;Done
GETVEC: ;Gets place for a vector result, and places a pointer on
;the interpreter stack. Location is returned in R0.
;Simple procedure.
MOV #VCTSPC,R0
JSR PC,GETSBK ;Allocate from small blocks
MOV R0,-(R3) ;Push new value cell pointer on interpreter stack.
EVSIG SBEVT ;End of critical section
RTS PC ;Done
GETTRN: ;Gets place for a trans result, and places a pointer on
;the interpreter stack. Location is returned in R0.
;Simple procedure.
MOV #TRNSPC,R0
JSR PC,GETSBK ;Allocate from small blocks
MOV R0,-(R3) ;Push new value cell pointer on interpreter stack.
EVSIG SBEVT ;End of critical section
RTS PC ;Done
;Variable declaration: MVAR, KVAR;
MVAR: ;Interpreter routine
COMMENT ⊗ A list of arguments, each of which is a type/count pair. This list
is terminated by a zero entry. For each data type entries in the environment
are created for the specified number of variables. Algebraic variables (scalar,
vector & trans) initially have no value. Events get created and their identifiers
are stored in the environment. For cmons a new condition monitor of the indicated
type is created and a pointer to its control block is placed in the environment.
Frame headers are created by AFFIX. Arrays are allocated when created. Note that
for arrays the count field actually contains the # of dimensions. For procedures
a procedure descriptor is created. ⊗
MOV ENV(R4),R2 ;R2 ← LOC[current environment]
MOV LVARS(R2),R2 ;R2 ← LOC[first free entry in environment]
1$: FETCH R1 ;R1 ← data type of variables to make
TST R1
BNE 2$ ; if any
MOV ENV(R4),R0 ;R0 ← LOC[environment]
MOV R2,LVARS(R0) ;Update first free variable entry
CCC
RTS PC ;(who says returns should go at the end?)
2$: FETCH R0 ;Get count of # of this type of variable to make
CMP R1,#EVNTYP ;See what data type they should be
BEQ 4$ ; event
BGT 5$ ; cmon, array & procedure
3$: MOV R1,(R2)+ ;For algebraic just stick data type in place
CLR (R2)+ ; & zero the value pointer
SOB R0,3$ ; for each one
BR 1$
4$: MOV R1,(R2)+ ;Set data type to event
EVMAK ;Make a new event
POP <(R2)+> ; & store its identifier in the environment
SOB R0,4$ ; for each one
BR 1$
5$: BIT #177400,R1 ;Check datatype
BNE 6$ ; array or procedure
JSR PC,CMMAK ;Make the new cmons & store them in environment
BR 1$
6$: BIT #ARYTYP,R1
BNE 10$ ; array
MOV R1,(R2)+ ;Set access type to procedure
PUSH <R0>
FETCH R0 ;Get # of arguments for procedure
PUSH <R0> ;Save it for later
ADD #PRODSZ,R0 ;R0 ← size of procedure descriptor (in words)
JSR PC,GTFREE ;R0 ← LOC[procedure descriptor]
MOV R0,(R2)+ ;Store pointer to procedure descriptor in env
FETCH (R0)+ ;Get IPC of procedure body
MOV ENV(R4),(R0)+ ;Set up pointer to environment outside procedure
MOV LEV(R4),(R0) ;Set up lexical level of procedure
INC (R0)+ ; to one greater than creating interpreter
FETCH (R0) ;Get size of environment needed
ASL (R0) ;Need 2 words/variable
ADD #ENVSIZ,(R0)+ ;Add in header size
POP <R1> ;Retrieve # of args
MOV R1,(R0)+ ; & save it in procedure descriptor
BEQ 8$ ; if any
7$: FETCH (R0)+ ;Get the access/datatype info for each arg
SOB R1,7$
8$: POP <R0> ;Restore count of procedures to make
SOB R0,6$ ; and make them all
BR 1$
10$: MOV R1,(R2)+ ;Indicate that this is an array
PUSH <R2,R1,R0> ;Save # of dimensions & type of array to make
ASL R0 ;Will need two words per subscript
JSR PC,GTFREE ;Get some temporary space to store bounds info
MOV R0,R2 ;R2 ← LOC[temp storage]
MOV (SP),R0 ;R0 ← # of dimensions in array
PUSH <#2> ;(SP) ← size needed for array (2 words/entry)
11$: PUSH <R0>
JSR PC,GTBND ;Get upper bound
JSR PC,GTBND ;Get lower bound
POP <R0>
MOV -4(R2),R1 ;Compute upper-lower+1
SUB -2(R2),R1
INC R1
MUL (SP),R1 ;(upper-lower+1) * size
MOV R1,(SP) ;Update array size
SOB R0,11$ ;Handle all of the subscript bounds
POP <R0> ;R0 ← array environment size
MOV (SP),R1 ;R1 ← # of dimensions in array
MUL #3,R1 ;Need three words per dimension
INC R1 ;An extra word for the # of dimensions
ADD R1,R0 ;R0 ← Size for array header + environment
ASL R1 ;R1 ← Size of header in bytes
PUSH <R1>
JSR PC,GTFREE ;R0 ← LOC[array header]
MOV R0,@6(SP) ;Store pointer to array header into environment
ADD (SP)+,R0 ;R0 ← LOC[beginning of environment]
MOV (SP),R1 ;R1 ← # of dimensions
PUSH <R0>
MOV #1,-(R0) ;Multiplier for nth subscript is one
12$: PUSH <R1>
MOV -(R2),-(R0) ;Put lower bound in place
MOV -(R2),R1 ;Get upper bound
MOV R1,-(R0) ;Put it away
SUB 2(R0),R1
INC R1 ; upper - lower + 1
MUL 4(R0),R1 ;Compute next multiplier
MOV R1,-(R0) ; & store it
POP <R1>
SOB R1,12$ ;Repeat for each dimension of array
PUSH <R0>
MOV R2,R0 ;R0 ← LOC[temp storage]
JSR PC,RLFREE ;Deallocate temp storage
POP <R0,R1> ;R0 ← LOC[array header], R1 ← LOC[array env]
MOV (R0),R2 ;R2 ← Size of the array
POP <(R0)> ;Store # of dimensions into array header
POP <R0> ;R0 ← datatype
BIC #ARYTYP,R0
CMP #EVNTYP,R0 ;An event array?
BEQ 14$
13$: MOV R0,(R1)+ ;Store datatype
CLR (R1)+ ;Zero value pointer
SOB R2,13$ ;Do the whole array
BR 15$
14$: MOV R0,(R1)+ ;Store datatype
EVMAK ;Make the event
POP <(R1)+> ; & store it away
SOB R2,14$ ;Do the whole array
15$: POP <R2> ;Restore R2
TST (R2)+ ;R2 ← LOC[next environment entry]
JMP 1$ ;DONE!!! Go see about making more variables
GTBND:: FETCH R0 ;Get bound
TST R0 ;See if constant or variable (level-offset)
BMI 1$
JSR PC,GETARG ;R0 ← LOC[variable]
LDF @2(R0),AC0 ;AC0 ← variable value (better be a scalar)
STCFI AC0,(R2)+ ;Store the upper bound away
BR 2$
1$: ASL R0 ;Get rid of the constant bit
ASR R0 ;But be sure to preserve the correct sign
MOV R0,(R2)+
2$: RTS PC
KVAR: ;Interpreter routine
COMMENT ⊗ Given the number of variables to kill. The last ones in the
current environment are destroyed. For each frame an attempt is made to
validate any dependents first. ⊗
FETCH R1 ;Get # of variables to kill
KVAR0:: ;Entry point for procedure returns
MOV ENV(R4),R2 ;R2 ← LOC[environment]
MOV LVARS(R2),R2 ;R2 ← LOC[first free variable entry]
1$: SUB #4,R2 ;R2 ← LOC[variable to kill]
TSTB 1(R2) ;Test access method
BEQ 10$ ;Do the direct accesses below
BIT #REFTYP,(R2)+ ;An indirect reference?
BNE 12$ ; yes - don't do anything
BIT #HDRTYP,-(R2) ;A frame?
BEQ 2$
TST (R2)+
JSR PC,KFRAME ;Kill the frame & its affixments
BR 12$
2$: BIT #PROTYP,(R2) ;A procedure descriptor?
BEQ 3$
TST (R2)+
PUSH <R1>
MOV (R2),R0
JSR PC,RLFREE ;Deallocate the procedure descriptor
POP <R1>
BR 12$
3$: BIT #ARYTYP,(R2)+ ;An array?
BEQ 12$ ; no - not an access method that needs killing
PUSH <R1,R2>
MOV (R2),R2 ;R2 ← LOC[array header]
MOV (R2)+,R1 ;R1 ← # of dimensions
MUL #6,R1 ;R1 ← # bytes taken up by bounds info
ADD R2,R1
MOV R1,R0 ;R0 ← LOC[beginning of array environment]
MOV (R2)+,R1 ;R1 ← upper bound of 1st subscript
SUB (R2)+,R1
INC R1 ;R1 ← range of 1st subscript
MUL (R2)+,R1 ;R1 ← size of array = # of entries to kill
MOV R0,R2 ;R2 ← LOC[first entry of array environment]
CMPB #TRNTYP,(R2) ;Check datatype of array
BGT 9$ ;Don't bother killing scalars & vectors
BEQ 5$
4$: TST (R2)+ ;Skip over type word
EVKIL (R2)+ ;Kill the event
SOB R1,4$ ;Get all of them
BR 9$
5$: BIT #HDRTYP,(R2)+ ;Do we have a frame header to kill?
BEQ 6$
JSR PC,KFRAME ;Kill the frame & its affixments
6$: TST (R2)+ ;Point to next environment entry
SOB R1,5$ ;Kill everyone
9$: POP <R2> ;Restore old environment pointer
MOV (R2),R0
JSR PC,RLFREE ;Deallocate array header
POP <R1> ;Restore old kill count
BR 12$
10$: CMP (R2)+,#EVNTYP ;What type is it?
BLT 12$ ;Algebraic types are easy
BGT 11$
EVKIL (R2) ;Kill the event
BR 12$
11$: JSR PC,CMDEST ;Kill the cmon
12$: CLR (R2) ;Zero the pointer field
CLR -(R2) ;Zero the type field
DEC R1
BGT 1$ ;Kill all that we were asked to
MOV ENV(R4),R0 ;R0 ← LOC[environment]
MOV R2,LVARS(R0) ;Update first free variable entry
CCC ;Clear condition code
RTS PC ;Done
;Stack ops: GTVAL, CHNGE, PUSHV
GTVAL:
COMMENT ⊗ The argument is a level-offset pair. The variable
referenced by that pair is examined and a pointer to its value cell
is placed on the stack. ⊗
FETCH R0 ;Pick up level-offset name of argument
GVAL0: ;HN Entry point for DOEVAL routine in PNTAID.PAL
JSR PC,GETARG ;R0 ← LOC[variable environment entry]
GVAL1: ;MSM entry point for POINTY routine GTARR in PINTRP.PAL
BIT #HDRTYP,(R0);Check access method
BNE 1$
MOV 2(R0),-(R3) ;Direct - push value pointer on stack
BNE 4$ ;If it had a value all done
CMP #TRNTYP,(R0)
BEQ 2$ ;Use niltrans for default
MOV #NILVEC,(R3);Use vector/scalar zero
BR 3$
1$: MOV 2(R0),R0 ;R0 ← LOC[frame header]
JSR PC,NOCMP ;Don't compact for a bit
CALL GETVAL,<R0>;R0 ← value
MOV R0,-(R3) ;Push value on interpreter stack.
JSR PC,YESCMP ;OK to compact now
TST (R3) ;Check that we got a valid value
BNE 4$
2$: MOV #NILTRN,(R3);If not use the niltrans
3$: ALERR GTVMES ; & complain
4$: CCC ;Clear condition code.
RTS PC ;Done
DATA
GTVMES::ASCIE </NO VALUE FOR VARIABLE - USING DEFAULT./>
CHNERR::ASCIE </CANNOT ASSIGN VALUES TO DEVICES OR FRAMES AFFIXED TO DEVICES./>
NILTRN:
NILROT: .FLT2 1.0, 0.0, 0.0 ;Define the default values
ZERO: .FLT2 0.0, 1.0, 0.0 ;XHAT,YHAT,ZHAT labels added by MSM 12/11/78
.FLT2 0.0, 0.0, 1.0
NILVEC: .FLT2 0.0, 0.0, 0.0
CODE
CHNGE:
COMMENT ⊗ Pops the value from top of stack into the variable specified
by the level-offset pair given in the argument. ⊗
FETCH R0 ;Pick up level-offset name of argument
CHNG0: ;MSM label for POINTY
JSR PC,GETARG ;R0 ← LOC[variable environment entry]
CHNG1: ;MSM label for POINTY
BIT #HDRTYP,(R0);Chech access method
BNE 1$
MOV (R3)+,2(R0) ;Direct - store value pointer away
BR 5$
1$: MOV 2(R0),R0 ;R0 ← LOC[Desired frame header]
BIT #FTYPE,TYPE(R0) ;Can't change values of devices
BEQ 2$ ;It's a device - go complain
BIT #DYNAM,TYPE(R0) ;Better not be a dynamic frame either
BEQ 3$ ;It's a regular frame - go change it
2$: ALERR CHNERR ;Can't change value of device or dynamic frame
BR 4$
3$: JSR PC,NOCMP ;Don't compact for a bit
CALL CHANGE,<R0,(R3)>
JSR PC,YESCMP ;OK to compact now
4$: TST (R3)+ ;Pop stack
5$: CCC ;Clear condition code.
RTS PC ;Done
PUSHV: FETCH -(R3) ;Put argument directly on stack
CCC ;Clear condition code.
RTS PC ;Done
;Flow-of-control: PROC, RETURN
PROC:
COMMENT ⊗ The arguments are the level-offset of the procedure being called, and
the list of parameters for the procedure. The procedure's arguments can come in
three flavors: on the R3 stack for expressions, an address pointing to a constant
or a level-offset for a variable. Variables can be passed either by value or
reference. Arrays are always passed by reference. This routine creates a new
procedure header, switches contexts (environments), allocates an environment
for the procedure & binds the arguments. When done R5 will point to the new
procedure header, so RETURN will know who to return from. ⊗
FETCH R0 ;Get level-offset of procedure descriptor
JSR PC,GETENV
MOV 2(R0),R2 ;R2 ← LOC[procedure descriptor]
MOV #PROHSZ,R0
JSR PC,GTFREE ;R0 ← LOC[new procedure header]
MOV R5,OR5(R0) ;Save old R5 value
MOV R0,R5 ;R5 ← LOC[currently active procedure]
MOV (R4),OIPC(R5) ;Save IPC of caller (points to arg list)
MOV (R2)+,(R4) ;Set up IPC of procedure body
MOV (R2)+,OENV(R5) ;temporarily store environment of proc's parent
MOV (R2)+,OLEV(R5) ;Lexical level of procedure
MOV (R2)+,R0 ;R0 ← Size of environment needed by procedure
JSR PC,GTFREE ;R0 ← LOC[environment for procedure]
MOV OENV(R5),(R0) ;SLINK ← environment of procedure's parent block
MOV R0,OENV(R5)
CMP (R0)+,(R0)+ ;R0 ← LOC[first entry in environment]
PUSH <(R2),R0> ;Save # of args & LOC[first entry]
MOV (R2)+,R1 ;R1 ← # of arguments for procedure
BEQ 2$ ; if any
1$: MOV (R2)+,(R0)+ ;Copy type info from procedure descriptor → env
TST (R0)+ ;Skip over value pointer
SOB R1,1$ ;Get all of them
2$: POP <R2> ;R2 ← LOC[first environment entry]
MOV R0,-2(R2) ;LVARS ← LOC[first free entry]
JSR PC,NOCMP ;Don't compact for a while
POP <R1> ;R1 ← # of arguments for procedure
BEQ 11$ ; if any
3$: PUSH <R1>
MOV @OIPC(R5),R0 ;R0 ← next arg
ADD #2,OIPC(R5) ;Bump IPC
CMP #-1,R0 ;See if stack reference
BNE 4$
BIC #REFTYP,(R2)+ ;Make it call by value for expressions
MOV (R3)+,(R2)+ ;Pop value off of stack into environment
BR 10$
4$: BIT #140000,R0 ;Check if label - NOTE: this hack won't work if
BEQ 5$ ; the PCODE is ever put below 40 000
BIC #REFTYP,(R2)+ ;Make it call by value for constants
MOV R0,(R2)+ ;Store constants address in environment
BR 10$
5$: BIT #ARYTYP,(R2) ;See if it's an array reference
BEQ 6$
JSR PC,GETENV ;R0 ← LOC[environment entry for array variable]
BIS #REFTYP,(R2)+ ;Arrays are always passed by reference
MOV R0,(R2)+ ;Store pointer to array variable
BR 10$
6$: JSR PC,GETARG ;R0 ← LOC[environment entry for variable]
BIT #REFTYP,(R2)+ ;Call by value or reference?
BEQ 7$
MOV R0,(R2)+ ;Call by reference - store LOC[var env entry]
BR 10$
7$: BIT #HDRTYP,(R0)+ ;Call by value - see if direct access or header
BNE 8$
MOV (R0),(R2)+ ;Direct - store value pointer
BR 10$
8$: CALL GETVAL,<(R0)> ;Header - get frame's value
MOV R0,(R2)+ ;Store value pointer
10$: POP <R1>
SOB R1,3$ ;Bind each argument of procedure
11$: EVWAIT INTEVT
MOV PROLST,NXTPRO(R5) ;Link us onto active procedure list
MOV R5,PROLST
MOV R3,OR3(R5) ;Save stack so we can restore it on return
MOV 4(SP),OSTOP(R5) ;Save old stack limits: overflow
MOV 2(SP),OSBOT(R5) ; + underflow
MOV #INSTSZ,R0 ;R0 ← Size needed for an interpreter stack
JSR PC,GTFREE ;R0 ← LOC[new interpreter stack]
MOV R0,STKBAS(R5) ;Store away new stack base
MOV R0,4(SP) ;So INT can check for stack overflow
ADD #2*INSTSZ,R0 ;R0 ← LOC[top of new stack] (INSTSZ is in bytes)
MOV R0,2(SP) ;So INT can check for stack underflow
MOV R0,R3 ;R3 ← top of new stack
MOV OENV(R5),R1
MOV ENV(R4),OENV(R5) ;Swap out caller's environment
MOV R1,ENV(R4) ; & swap in procedure's
MOV OLEV(R5),R1
MOV LEV(R4),OLEV(R5) ;Ditto with lexical levels
MOV R1,LEV(R4)
EVSIG INTEVT
JSR PC,YESCMP ;Okay to compact now
CCC
RTS PC ;Done - Go interpret the procedure.
RETURN:
COMMENT ⊗ When returning from a procedure kill all of the local variables in
the procedure's environment, release the environment, & swap back in the
environment of the caller. Also remove procedure from PROLST & deallocate
the procedure header. ⊗
TST R5 ;See if R5 points to a procedure header
BEQ 10$ ; Doesn't point anywhere!
CMP -2(R5),#-<2*PROHSZ+4> ;Is the block the right size?
BEQ 1$
10$: ALERR RETMES ;No!!! - we're in big trouble now!!!
BMPIPC ;Skip over value to return flag
RTS PC ;Just ignore the return if user proceeds - good luck
1$: MOV ENV(R4),R0 ;R0 ← LOC[procedure's environment]
TST (R0)+ ;R0 ← LOC[LVARS]
MOV (R0)+,R1 ;R1 ← LOC[first free slot in environment]
SUB R0,R1 ;Length of used entries in bytes
ASH #-2,R1 ;Divide by four to get # of environment entries
BEQ 2$ ;If any
JSR PC,KVAR0 ;Go kill all the procedure's local variables
2$: MOV ENV(R4),R0
JSR PC,RLFREE ;Release the procedure's environment
FETCH R0 ;See if we return a value
TST R0
BEQ 3$
MOV (R3)+,R0 ;Get the pointer to the value
3$: MOV OR3(R5),R3 ;restore old R3 stack
TST R0
BEQ 4$
MOV R0,-(R3) ;Push return value on stack
4$: MOV STKBAS(R5),R0
JSR PC,RLFREE ;Release the procedure's stack
MOV OENV(R5),ENV(R4) ;Restore environment of caller
MOV OLEV(R5),LEV(R4) ; and lexical level
MOV OIPC(R5),(R4) ; IPC too
MOV OSTOP(R5),4(SP) ;Restore old stack limits: overflow
MOV OSBOT(R5),2(SP) ; + underflow
EVWAIT INTEVT
MOV #PROLST,R0 ;Remove us from active procedure list
5$: CMP (R0),R5 ;Found us yet?
BEQ 6$
MOV (R0),R0 ;Try next
BNE 5$
BR 7$ ;We weren't there!
6$: MOV (R5),(R0) ;Unlink us
7$: EVSIG INTEVT
MOV R5,R0
MOV OR5(R5),R5 ;Restore old R5 value
JSR PC,RLFREE ;Release procedure header
CCC
RTS PC ;Done - return to caller
DATA
RETMES:: ASCIE </No where to RETURN to!!! - continuing will ignore RETURN/>
CODE
; ABORT, GODDT, NOOP, FORCHK, FOREND, CASE, JUMP, JUMPC
ABORT:
;Aborts current motions
;This should be cleaned up sometime.
MOV #20,R1 ;First stop everyone: 2 arms (6 jts ea), 2 hands & 2 devices
MOV LDVCPTR,R0 ;R0 ← LOC[table of device pointers]
1$: MOV (R0)+,R2 ;R2 ← device block
BEQ 2$ ;If any
BIS #100000,@0(R2) ;Stop this device.
2$: SOB R1,1$ ;Repeat till all devices stopped
;SLEEP #144 ;Should pause for a bit (1/10 sec) here but...
; if anything gets printed no problem
CCC ;Clear the condition codes
RTS PC ; & Return
GODDT: BPT ;break to DDT
NOOP: CCC ;Clear the condition codes
RTS PC ; and Return
FORCHK: ; change parallel routine in PINTRP.PAL when you change this
;Assume that the stack has, from surface in, the increment, the
; final value, and the control variable's value, all of which are
; scalar values. If (FINAL-CONVAR)*(INCREMENT) ≥ 0 then this is a
; no-op; otherwise, jump to the destination (end of FOR body) & clean up stack
;Arguments: destination.
FETCH R0 ;Pick up level-offset name of control variable
JSR PC,GETARG ;R0 ← LOC[variable environment entry]
MOV 4(R3),2(R0) ;Store pointer to current value
LDF @2(R3),AC0 ;AC0 ← final value
SUBF @4(R3),AC0 ;AC0 ← final - current
MULF @(R3),AC0 ;AC0 ← (final - current)*increment
FETCH R0 ;R0 ← destination
CFCC
BGE 1$ ;Shall this be a no-op?
MOV R0,IPC(R4) ;No; set new IPC.
ADD #6,R3 ;Pop the inc, final & control var off of the stack
1$: CLR R0
RTS PC ;Done
FOREND: ;Interpreter routine
;Assume that the stack has, from surface in, the increment, the
; final value, and the control variable's value, all of which are
; scalar values. Copy the step size and the current value, add them
; and replace the current value. Then jump to the start of the loop.
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3),-(R3) ;Copy step size
MOV 6(R3),-(R3) ;Copy current value
JSR PC,SADD ;Add them
MOV (R3)+,4(R3) ;Update the current value
JSR PC,YESCMP ;Okay to compact again
BR JUMP ;Now jump to start of for loop
CASE: ;Interpreter routine
;CASE statement. Case index on stack. Takes two arguments: the permisible
;range of the index & a list of addresses telling where each of the
;statements starts.
LDF @(R3)+,AC0 ;Fetch case index
FETCH R2 ;R2 ← error index
STCFI AC0,R0 ;R0 ← case index converted to integer
BPL 1$
MOV R2,R0
NEG R0 ;Check if there's an ELSE statement (error index < 0)
BPL 3$ ; yup - go do ELSE
ALERR 4$ ;Case index is negative - complain
MOV R2,R0
BR 3$ ;Go skip to end of the case statement
1$: MOV R2,R1 ;R1 ← Maximum legal index
BPL 2$
NEG R1 ;Make sure we use |range|, it's negative if ELSE stmnt
2$: CMP R0,R1 ;Check that index is within legal range
BLT 3$
MOV R2,R0
NEG R0 ;Check if there's an ELSE statement (error index < 0)
BPL 3$ ; yup - go do ELSE
ALERR 5$ ;Case index is too large - complain
MOV R2,R0 ;Go skip to end of the case statement
3$: ASL R0 ;Convert R0 to byte offset
ADD R0,(R4) ;IPC points to address of statement to interpret
MOV @(R4),(R4) ;Jump to proper statement
CCC ;Clear condition code.
RTS PC ;Done
DATA
4$: .ASCIZ /CASE INDEX NEGATIVE/
5$: ASCIE /CASE INDEX TOO LARGE/
CODE
JUMP:
;Takes one argument: the new address.
MOV @(R4),(R4) ;Jump to new address
CCC ;Clear condition code.
RTS PC ;Done
JUMPC: ;Interpreter routine
COMMENT ⊗ Takes one argument: the destination address.
The condition queries the top of the stack and pops it, assuming it
to be a scalar. The interpreter jumps to the destination address if
the value of the scalar is false (0). rewritten 9-14-76 by arg ⊗
LDF @(R3)+,AC0 ;Get value of boolean
CFCC ;copy condition codes
BEQ 1$ ;if false succeed - take branch
BMPIPC ;skip over address
RTS PC ; & return
1$: MOV @IPC(R4),IPC(R4); branch
RTS PC ; & return
; SPAWN, SPROUT, TERMINATE
SPAWN: ;Utility routine
COMMENT ⊗ Takes two arguments: In R0, the IPC of the interpreter to
spawn, and in R1, the event (if any) to put in EVT of the new
interpreter. The inferior will have the same environment as the
superior. Creates an interpreter status block, stack, process
descriptor, and is ready for a SCHEDU when it returns the process
descriptor in R0. ⊗
PUSH <R1,R0> ;Save the EVT & the new IPC
MOV #ISBS,R0 ;R0 ← Size (in words) of an interpreter status block
JSR PC,GTFREE ;R0 ← LOC[new interpreter status block]
POP <IPC(R0)> ;new IPC ← first argument
MOV ENV(R4),ENV(R0) ;new ENV ← old ENV
MOV LEV(R4),LEV(R0) ;new LEV ← old LEV
EVWAIT INTEVT ;Interlock sensitive operation.
MOV #NXTINT+ISTBLK,R1 ;Link into the interpreter list.
MOV (R1),NXTINT(R0)
MOV R0,(R1)
EVSIG INTEVT ;End of interlock
POP <EVT(R0)> ;new EVT ← second argument.
PUSH <R0> ;Save LOC[new interpreter status block]
MOV #INSTSZ,R0 ;R0 ← Size needed for an interpreter stack
JSR PC,GTFREE ;R0 ← LOC[new interpreter stack]
POP <R1> ;R1 ← LOC[new interpreter status block]
MOV R0,STKBAS(R1) ;Store away new stack base
ADD #2*INSTSZ,R0 ;R0 ← LOC[top of new stack] (INSTSZ is in bytes)
PUSH <R1,R0> ;Save R1 & R0
MOV #210,R0 ;Room for process descriptor
JSR PC,GTFREE ;R0 ← LOC[new process descriptor]
MOV #UFPUSE+UGRSAV+2,PDBSTA(R0) ;Use floating point,saved registers, pri=1
POP <PDBR3(R0),R1> ;Store away new interp stack pointer (reg 3)
;R1 ← LOC[new ISB]
MOV R0,PDB(R1) ;Store away LOC[PDB] in new ISB
MOV R1,PDBR4(R0) ;Store away LOC[ISB] in reg 4 of PDB
MOV R0,USKMIN(R0) ;Set up min pointer for SP
ADD #UFEC+36,USKMIN(R0)
MOV R0,USKMAX(R0) ;Set up max pointer for SP
ADD #420,USKMAX(R0)
MOV #144040,UPSW(R0) ;Set up psw
MOV PDB(R4),R1 ;Use same UIMAP that we are using.
MOV UIMAP(R1),UIMAP(R0)
RTS PC ;Done
; These are the appropriate scheduling commands:
; SCHEDU R0,#INTERP,#USRDM,#2 ;Cause the new process to be started, suspended
; FORK R0,#INTERP,#USRDM ;Cause the new process to be started.
SPROUT: ;Interpreter routine
COMMENT ⊗ Arguments: One address in pseudo-code for each of the
several forks starting up, followed by a 0 word. This is to be used
only for cobegins, not for servos. Each new interpreter is given an
interpreter status block and is then scheduled. As each terminates,
it signals its defining event. Since each of these has the same
event, the current interpreter need only wait until they all happen.
⊗
PUSH <R3> ;Save R3. Caution: cannot use interpreter stack now.
CLR R3 ;R3 is the count of how many inferiors to spawn.
EVMAK ;-(SP) ← Event identifier for communication with infs.
1$: FETCH R0 ;R0 ← next argument (IPC)
TST R0
BEQ 2$ ;If zero, then we have spawned all the inferiors.
INC R3 ;Count it.
MOV (SP),R1 ;R1 ← event for the inferior EVT
JSR PC,SPAWN
MOV R0,R2 ;R2 ← new process control block
;Set up the new environment
JSR PC,NEWENV ;R0 ← LOC[new environment]
MOV ENV(R4),SLINK(R0) ;Not necessary to set up OLEV, etc.
MOV PDBR4(R2),R1
MOV R0,ENV(R1)
INC LEV(R1)
SCHEDU R2,#INTERP,#USRDM,#2 ;Cause the new process to be started, suspended
BR 1$ ;Go handle the next inferior.
2$: DEC R3 ;Another wait to be done?
BMI 3$ ;No, we are finished.
EVWAIT (SP) ;Wait for an inferior to come back.
BCC 2$ ;If all well, wait for the next one.
ALERR SPRMES ;The event was killed!
3$: EVKIL (SP)+ ;Kill the event now, remove from stack
POP <R3> ;Restore R3
CCC ;Clear condition code.
RTS PC ;Done
DATA
SPRMES::ASCIE /BAD RETURN FROM INFERIOR/
CODE
TERMINATE:
COMMENT ⊗ Interpreter routine, sometimes jumped to from other
interpreter routines. End this interpreter. ⊗
MOV EVT(R4),R0 ;R0 ← event to announce imminent demise
BEQ 1$ ;If there is one
EVSIG R0 ;Announce that we are about to disappear.
1$: MOV STKBAS(R4),R0 ;Reclaim interpreter stack
JSR PC,RLFREE
MOV ENV(R4),R0 ;Reclaim this environment
JSR PC,RLFREE
PUSH <PDB(R4)> ;Save LOC[this PDB]
MOV R4,R0 ;Reclaim Interpreter Status Block
JSR PC,RLFREE
EVWAIT INTEVT ;Enter critical region.
MOV #ISTBLK,R0 ;The following unlinks this interpreter from the chain.
2$: MOV R0,R1
MOV NXTINT(R1),R0
CMP R0,R4 ;Have we found ours yet?
BNE 2$
MOV NXTINT(R4),NXTINT(R1) ; Yes. rechain.
EVSIG INTEVT ;Leave critical region.
POP <R0> ;Reclaim process control block (should be safe now)
CMP R0,FREEST ;Make sure that it points into free storage.
BLE 3$ ; (it may be statically allocated)
CMP R0,#FREEND
BGE 3$
JSR PC,RLFREE
3$: DISMIS ;Go away
;Booleans: SLE,SLT,SGE,SGT,SEQ,SNE,AND,LOR,NOT,LXOR,EQV
COMP: ;auxiliary function used by SLE,SLT,SGE,SGT,SEQ,SNE
LDF @(R3)+,AC0 ;Get first arg
CMPF @(R3)+,AC0 ;Compare it with second arg (1st-2nd)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC, GETSCA ;R0 ← -(R3) ← LOC[new_scalar]
MOV ONE,(R0)+ ;assume true (1.0)
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CFCC ;copy condition flags from compare
RTS PC ; & Return
SLT: JSR PC,COMP ;compare the args
BLT 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
SLE: JSR PC,COMP ;compare the args
BLE 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
SGT: JSR PC,COMP ;compare the args
BGT 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
SGE: JSR PC,COMP ;compare the args
BGE 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
SEQ: JSR PC,COMP ;compare the args
BEQ 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
SNE: JSR PC,COMP ;compare the args
BNE 1$ ;if true then done
CLR @(R3) ;else set answer to false (0)
1$: RTS PC ; & return
AND: LDF @(R3)+,AC0 ;Get first arg
LDF @(R3)+,AC1 ;Get second arg (and set condition flags)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC, GETSCA ;R0 ← -(R3) ← LOC[new_scalar]
CLR (R0)+ ;assume false (0)
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CFCC ;copy condition flags for 2nd arg
BEQ 1$ ;if it's false return false
TSTF AC0 ;else look at 1st arg
CFCC
BEQ 1$
MOV ONE,@(R3) ;if both args are true return true (1.0)
1$: RTS PC ; Return
LOR: LDF @(R3)+,AC0 ;Get first arg
LDF @(R3)+,AC1 ;Get second arg (and set condition flags)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC, GETSCA ;R0 ← -(R3) ← LOC[new_scalar]
MOV ONE,(R0)+ ;assume true (1.0)
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CFCC ;copy condition flags from compare
BNE 1$ ;if it's true return true
TSTF AC0 ;else look at 1st arg
CFCC
BNE 1$
CLR @(R3) ;if both args are false return false (0)
1$: RTS PC ; Return
NOT: LDF @(R3)+,AC0 ;Get arg (and set condition flags)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC, GETSCA ;R0 ← -(R3) ← LOC[new_scalar]
CLR (R0)+ ;assume false (0)
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CFCC ;copy condition flags for arg
BNE 1$ ;if it's false return true
MOV ONE,@(R3) ; else return true
1$: RTS PC ; Return
EQV: LDF @(R3)+,AC0 ;Get first arg
LDF @(R3)+,AC1 ;Get second arg (and set condition flags)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC, GETSCA ;R0 ← -(R3) ← LOC[new_scalar]
MOV ONE,(R0)+ ;assume true (1.0)
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CFCC ;copy condition flags from compare
BEQ 1$ ;if it's true check that other is
TSTF AC0 ;2nd arg is false look if 1st arg is too
CFCC
BNE 2$ ;Nope not both false
BR 3$ ;Yup both false
1$: TSTF AC0 ;2nd arg is true look if 1st arg is too
CFCC
BNE 3$ ;Both are true
2$: CLR @(R3) ;if both args aren't the same return false (0)
3$: RTS PC ; Return
LXOR: JSR PC,EQV ;Compute equivalence relation of the 2 args
JMP NOT ; & negate it (⊗ = ¬≡), then return
;return scalars: SABS,SADD,SSUB,SMUL,SDIV,SNEG,SEXP,MAX,MIN,INT,IDIV,MOD
COMMENT ⊗ All timings are averages of 1000 runs. They take into
account the cost of the RTS but not the JSR. It is assumed that
GETSCA and GETVEC take no time. All routines on this page are
interpreter routines. ⊗
SABS: ;Scalar ← |Scalar|
LDF @(R3)+,AC0 ;AC0 ← arg
ABSF AC0 ;AC0 ← |arg|
BR SRET ;Store result & return
;30 microseconds
SADD: ;Scalar ← Scalar + Scalar
LDF @(R3)+,AC0 ;AC0 ← arg 2
ADDF @(R3)+,AC0 ;AC0 ← arg2 + arg1
BR SRET ;Store result & return
SSUB: ;Scalar ← Scalar - Scalar
LDF @2(R3),AC0 ;AC0 ← arg 1
SUBF @(R3)+,AC0 ;AC0 ← arg1 - arg2
TST (R3)+ ;Move past first argument
BR SRET ;Store result & return
;30 microseconds
SMUL: ;Scalar ← scalar * scalar
LDF @(R3)+,AC0 ;AC0 ← arg 2
MULF @(R3)+,AC0 ;AC0 ← arg2 * arg1
BR SRET ;Store result & return
;33 microseconds
SDIV: ;Scalar ← Scalar / Scalar
LDF @(R3)+,AC1 ;AC1 ← arg 2
LDF @(R3)+,AC0 ;AC0 ← arg 1
DIVF AC1,AC0 ;AC0 ← arg1 / arg2
BR SRET ;Store result & return
;26 microseconds
SNEG: ;Scalar ← -Scalar
LDF @(R3)+,AC0 ;AC0 ← arg
NEGF AC0 ;AC0 ← -arg
BR SRET ;Store result & return
INT: LDF @(R3)+,AC0 ;AC0 ← arg
STCFI AC0,R0 ;R0 ← Integer part of arg
LDCIF R0,AC0 ;Float the integer part
BR SRET
DIVMOD: LDF @(R3)+,AC0 ;AC0 ← 2nd arg
STCFI AC0,-(SP) ;push integer part of it
LDF @(R3)+,AC0 ;AC0 ← 1st arg
STCFI AC0,R0 ;R0 ← integer part of it
ASHC #-20,R0 ;R0,R1 ← 32 bit integer part of 1st arg
DIV (SP)+,R0 ;Divide 2nd arg into 1st. R0 ← quotient, R1 ← remainder
RTS PC
IDIV: JSR PC,DIVMOD ;Do common code
LDCIF R0,AC0 ; & float quotient
BR SRET
MOD: JSR PC,DIVMOD ;Do common code
LDCIF R1,AC0 ; & float remainder
SRET:: JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar block]
STF AC0,@(R3) ;Store result
CCC ;Clear condition code.
RTS PC ;Done
MAX: LDF @(R3)+,AC0 ;Get 2nd arg
CMPF @(R3),AC0 ;Compare 1st against 2nd
CFCC ;Copy condition codes. N set if 1st<2nd
BPL 1$
MOV -2(R3),(R3) ;2nd was max
1$: CCC
RTS PC ;Done
MIN: LDF @(R3)+,AC0 ;Get 2nd arg
CMPF @(R3),AC0 ;Compare 1st against 2nd
CFCC ;Copy condition codes. N set if 1st<2nd
BMI 1$
MOV -2(R3),(R3) ;2nd was min
1$: CCC
RTS PC ;Done
SEXP: ;Scalar ← Scalar ↑ Scalar
TSTF @2(R3) ;Check sign of base
CFCC
BGT 3$ ;Positive base - no sweat
BMI 2$ ;Negative base - see if exponent is integer or not
TST @(R3)+ ;Zero base - check sign of exponent
CLRF AC0 ; Result is zero if exponent is positive
BPL 1$
LDF INF,AC0 ; or +infinity if exponent is negative
1$: TST (R3)+ ;Fix up stack - pop base
BR SRET ;All done - return
2$: LDF @(R3),AC0 ;AC0 ← exponent
STCFI AC0,R0 ;R0 ← INT(exponent)
LDCIF R0,AC1 ;AC1 ← INT(exponent)
CMPF AC0,AC1 ;Is exponent an integer?
CFCC
BNE 3$ ; Nope - use Exp(B*Log(A)) to compute A↑B
BIT #1,R0 ;Check if integer exponent is odd
BEQ 3$ ; Nope - skip ahead
JSR PC,3$ ; Yup - raise to power
NEGF @(R3) ; then negate result
RTS PC ;All done
3$: LDF @2(R3),AC0 ;Pick up base
JSR PC,@LLOG ;AC0 ← Log(A)
MULF @(R3)+,AC0 ;AC0 ← B * Log(A) = Log(A↑B)
TST (R3)+ ;Fix up stack - pop base
JSR PC,@LEXP ;AC0 ← Exp(Log(A↑B)) = A↑B
BR SRET ; & Return
DATA
INF: .WORD 077777, 177777 ;Largest possible positive floating point number
CODE
; VDOT, VMAG, SSBRTN
;96 -- 116 microseconds
VDOT: ;Scalar ← Vector dot Vector
;S ← (X1X2 + Y1Y2 + Z1Z2) / W1W2
PUSH <R2> ;Save R2.
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3)+,R1 ;R1 ← LOC[arg 2]
MOV (R3)+,R0 ;R0 ← LOC[arg 1]
CLRF AC0 ;AC0 ← 0. Running total
MOV #3,R2 ;R2 ← 3: Length of vector
1$: LDF (R0)+,AC1 ;Form sum of products of first 3 terms
MULF (R1)+,AC1 ;
ADDF AC1,AC0 ;
SOB R2,1$ ;Loop until all 3 fields done.
DIVF (R0),AC0 ;Divide by W1
DIVF (R1),AC0 ;Divide by W2. AC0 now has answer.
JSR PC,YESCMP ;OK to compact now
POP <R2> ;Restore R2
BR SRET ;Store result & return
;199 -- 207 microseconds
VMAGN: ;Scalar ← Norm (vector)
;S ← SQRT(XX + YY+ ZZ) / W
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3)+,R1 ;R1 ← LOC[arg]
LDF (R1)+,AC0 ;AC0 ← X
MULF AC0,AC0 ;AC0 ← XX
LDF (R1)+,AC1 ;AC1 ← Y
MULF AC1,AC1 ;AC1 ← YY
ADDF AC1,AC0 ;AC0 ← XX + YY
LDF (R1)+,AC1 ;AC1 ← Z
MULF AC1,AC1 ;AC1 ← ZZ
ADDF AC1,AC0 ;AC0 ← XX + YY + ZZ
MOV R1,-(SP) ;Push LOC[W] onto system stack, to save across SQRTF
JSR PC,@LSQRTF ;AC0 ← SQRT(XX + YY + ZZ)
DIVF @(SP)+,AC0 ;AC0 ← AC0 / W
JSR PC,YESCMP ;OK to compact now
BR SRET ;Store result & return
SSBRTN: ;Call a routine.
LDF @(R3)+,AC0 ;AC0 ← arg
FETCH R0 ;R0 ← which routine (a small number)
ASL R0 ;Double (words → bytes)
BLE 1$ ;Too small.
CMP R0,#SBLSIZ ;Too large?
BGE 1$ ;Yes
JSR PC,@SBRLST(R0) ;Call a routine. AC0 ← answer.
BR SRET ;Store result & return
1$: ALERR SSBRMS ;Complain
SCC ;Set condition code
RTS PC ;Done
DATA
SSBRMS::ASCIE </NO SUCH SUBROUTINE/>
SBRLST: ;List of legal subroutines
0 ;Illegal
SQRT ;#1
SIN ;#2
COS ;#3
TAN ;#4
ASIN ;#5
ACOS ;#6
ATAN2 ;#7
LOG ;#8
EXP ;#9
RTIME ;#10
SBLSIZ == .-SBRLST ;The size of the list (bytes)
CODE
SQRT: JMP @LSQRTF ;Let it do the returning
SIN: JMP @LSNCSD ;Let it do the returning
COS: JSR PC,@LSNCSD
STF AC1,AC0
RTS PC
TAN: JSR PC,@LSNCSD
DIVF AC1,AC0 ;Tan = Sin / Cos
RTS PC
ASIN: JMP @LASIN ;Let it do the returning
ACOS: JMP @LACOS ;Let it do the returning
ATAN2: LDF @(R3)+,AC1 ;Get second argument for atan2(#1,#2)
JMP @LATAN2
LOG: JMP @LLOG ;Let it do the returning
EXP: JMP @LEXP ;Let it do the returning
RTIME: GETTIM ;Get current time from kernel
CLRB 1(SP) ;So we don't lose the lower bits on conversion
SETL ;Time is in long format
LDCLF (SP)+,AC1 ;AC1 ← time in msecs
SETI ;Back to normal short integer format
DIVF THOUS,AC1 ;AC1 ← time in seconds
SUBF AC0,AC1 ;Get elapsed time
STF AC1,AC0 ;Store it in AC0
RTS PC
;Vector utilities: UNITV, CROSV
;281 -- 286 microseconds
UNITV: ;Vector ← V / Norm(V)
;S ← SQRT(XX + YY+ ZZ)
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3),R1 ;R1 ← LOC[arg]
LDF (R1)+,AC0 ;AC0 ← X
MULF AC0,AC0 ;AC0 ← XX
LDF (R1)+,AC1 ;AC1 ← Y
MULF AC1,AC1 ;AC1 ← YY
ADDF AC1,AC0 ;AC0 ← XX + YY
LDF (R1)+,AC1 ;AC1 ← Z
MULF AC1,AC1 ;AC1 ← ZZ
ADDF AC1,AC0 ;AC0 ← XX + YY + ZZ
JSR PC,@LSQRTF ;AC0 ← SQRT(XX + YY + ZZ)
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector block]
MOV 2(R3),R1 ;R1 ← LOC[old vector]
MOV #3,R2 ;R2 ← count of fields
1$: LDF (R1)+,AC1 ;AC1 ← field of vector
DIVF AC0,AC1 ;divide by norm
STF AC1,(R0)+ ;Store result
SOB R2,1$ ;Loop until done
MOV ONE,(R0)+ ;Set W to 1
CLR (R0) ; (two words long)
MOV (R3)+,(R3) ;Fix-up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
;172 -- 184 microseconds
CROSV: ;Vector ← Vector cross Vector
;X ← Y1Z2 - Y2Z1
;Y ← X2Z1 - X1Z2
;Z ← X1Y2 - X2Y1
;W ← W1W2
;AC0, 1, 2, 3, 4, 5 are garbaged by this routine.
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector block]
MOV 2(R3),R2 ;R2 ← LOC[arg 2]
MOV 4(R3),R1 ;R1 ← LOC[arg 1]. Must not pop R3 stack yet!
LDF 14(R1),AC0 ;AC0 ← W1
MULF 14(R2),AC0 ;AC0 ← W1W2
STF AC0,14(R0) ;Store AC0 → W
LDF 4(R1),AC0 ;AC0 ← Y1
LDF (R2),AC1 ;AC1 ← X2
LDF 4(R2),AC2 ;AC2 ← Y2
LDF (R1),AC3 ;AC3 ← X1
STF AC3,AC4 ;AC4 ← X1
STF AC0,AC5 ;AC5 ← Y1
MULF AC2,AC3 ;AC3 ← X1Y2
MULF AC1,AC0 ;AC0 ← X2Y1
SUBF AC0,AC3 ;AC3 ← X1Y2 - X2Y1
STF AC3,10(R0) ;Z ← AC3
LDF 10(R2),AC0 ;AC0 ← Z2
LDF 10(R1),AC3 ;AC3 ← Z1
MULF AC4,AC0 ;AC0 ← X1Z2
MULF AC3,AC1 ;AC1 ← X2Z1
SUBF AC0,AC1 ;AC1 ← X2Z1 - X1Z2
STF AC1,4(R0) ;Y ← AC1
LDF 10(R2),AC0 ;AC0 ← Z2
MULF AC5,AC0 ;AC0 ← Y1Z2
MULF AC2,AC3 ;AC3 ← Y2Z1
SUBF AC3,AC0 ;AC0 ← Y1Z2 - Y2Z1
STF AC0,(R0) ;X ← AC0
MOV (R3)+,2(R3) ;Put result cell where first arg was
TST (R3)+ ; & fix-up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
;TRANS extraction routines: TPOS, TORIEN, TAXIS, TMAGN
TPOS: ;Extracts the position part of a TRANS (last column)
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[New Vector]
MOV 2(R3),R1 ;R1 ← LOC[TRANS]
ADD #44,R1 ;R1 ← LOC [last column of TRANS]
MOV #6,R2 ;Three 2-word components to move
1$: MOV (R1)+,(R0)+ ;Copy it
SOB R2,1$
MOV ONE,(R0)+ ;Stick in the scale factor
CLR (R0)
MOV (R3)+,(R3) ;Fix-up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition codes
RTS PC ; & Return
TORIEN: ;Extracts the rotation part of a TRANS
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[New TRANS]
MOV 2(R3),R1 ;R1 ← LOC[TRANS]
MOV #22,R2 ;Three columns to do, three 2-word #'s/col
1$: MOV (R1)+,(R0)+ ;Copy the ROTN
SOB R2,1$
MOV #6,R2
2$: CLR (R0)+ ;Set up last column, three 0's
SOB R2,2$
MOV (R3)+,(R3) ;Fix-up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear cond codes
RTS PC ; & Return
;TAXIS & TANGLE routines to extract the axis vector and angle of rotation
; given a rotation (trans);
;Define some constants
DATA
ONE: .FLT2 1.0
TWO: .FLT2 2.0
CTHIRD: .FLT2 0.576 ;Square root of 1/3
C1001: .FLT2 1.0001
C0001: .FLT2 0.0001
CODE
TAXIS: JSR PC,TAXAN ;Get vector components in AC3,AC4 & AC5
TST (R3)+ ;Fix stack
JSR PC,GETVEC ;Get a new vector to store results
STF AC3,(R0)+
LDF AC4,AC0
STF AC0,(R0)+ ;Store X,Y & Z components
LDF AC5,AC0
STF AC0,(R0)+
MOV ONE,(R0)+ ;Store scale factor of 1
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition codes
RTS PC ; & Return
TMAGN: JSR PC,TAXAN ;Get COS(angle) in AC0, vector components in AC 3-5
STF AC3,-(SP) ;Store X component
JSR PC,@LACOS ;Compute angle in AC0
LDF (SP)+,AC3 ;Retrieve X
LDF CTHIRD,AC1 ;Square root of 1/3
LDF AC3,AC2 ;Get X
ABSF AC2
CMPF AC2,AC1 ;ABS(X)-SQRT(1/3)
CFCC ;Copy FPP cond codes into CPU cond codes
BLT 1$
LDF 34(R2),AC1 ;Get (2,3)
SUBF 24(R2),AC1 ;(2,3) - (3,2)
MULF AC3,AC1 ;Get sign of SIN(angle)
BR 4$
1$: LDF AC4,AC2 ;Get Y
ABSF AC2
CMPF AC2,AC1 ;ABS(Y)-SQRT(1/3)
CFCC ;Copy FPP cond codes into CPU cond codes
BLT 2$
LDF 10(R2),AC1 ;Get (3,1)
SUBF 30(R2),AC1 ;(3,1) - (1,3)
MULF AC4,AC1 ;Get sign of SIN(angle)
BR 4$
2$: LDF AC5,AC2 ;Get Z
ABSF AC2
CMPF AC2,AC1 ;ABS(Z)-SQRT(1/3)
CFCC ;Copy FPP cond codes into CPU cond codes
BLT 3$
LDF 14(R2),AC1 ;Get (1,2)
SUBF 4(R2),AC1 ;(1,2) - (2,1)
MULF AC5,AC1 ;Get sign of SIN(angle)
BR 4$
3$: ALERR TMAGMS ;Complain
CLRF AC0 ;& return NILROT
4$: CFCC
BLT 5$
NEGF AC0 ;If SIN(angle) > 0 then negate angle
5$: TST (R3)+ ;Clean up stack
JSR PC,YESCMP ;OK to compact now
JSR PC,GETSCA ;Get a scalar
STF AC0,@(R3) ;Store the angle of rotation
CCC ;Clear condition codes
RTS PC ; & Return
DATA
TMAGMS::ASCIE </ROTATION STRANGENESS/>
CODE
TAXAN: ;Code common to both TAXIS & TMAGN
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3),R2 ;R2 points to the ROT
LDF (R2),AC0 ;(1,1)
ADDF 20(R2),AC0 ;(2,2)
ADDF 40(R2),AC0 ;AC0 ← [(1,1)+(2,2)+(3,3)-1]/2 = COS(angle)
SUBF ONE,AC0
STF AC0,AC3 ;we'll use this later
DIVF TWO,AC0
STF AC0,AC1 ;Make a copy
ABSF AC1
CMPF C1001,AC1 ;If ABS(COS(angle)) > 1.0001 return the NILROT
CFCC
BGT 1$ ;Else go and compute the axis of rotation
CLRF AC0
STF AC0,AC3
STF AC0,AC4 ;NILROT = 0 degrees about (0,0,1)
LDF ONE,AC1
STF AC1,AC5
RTS PC
1$: STF AC0,-(SP) ;Store COS(angle) away for later
NEGF AC3
ADDF TWO,AC3 ;AC3 ← 3 - (1,1) - (2,2) - (3,3)
LDF ONE,AC0
SUBF (R2),AC0 ;(1,1)
SUBF 20(R2),AC0 ;(2,2)
ADDF 40(R2),AC0 ;(3,3)
DIVF AC3,AC0 ;AC0 ← Z↑2
CMPF C0001,AC0
CFCC
BLT 3$ ;If Z > 0.0001 skip ahead
CLRF AC5 ;Set Z ← 0
LDF ONE,AC0
SUBF (R2),AC0 ;(1,1)
ADDF 20(R2),AC0 ;(2,2)
SUBF 40(R2),AC0 ;(3,3)
DIVF AC3,AC0 ;AC0 ← Y↑2
CMPF C0001,AC0
CFCC
BLT 2$ ;If Y > 0.0001 skip ahead
CLRF AC4 ;Set Y ← 0
LDF ONE,AC3 ;Set X ← 1
BR 5$ ;Skip to end
2$: JSR PC,@LSQRTF ;Get SQRT(Y↑2)
STF AC0,AC4
LDF AC5,AC2 ;Clear this for later
BR 4$ ;Skip ahead to where X is computed
3$: JSR PC,@LSQRTF ;Get SQRT(Z↑2)
STF AC0,AC5
LDF ONE,AC2
STF AC2,AC3 ;For later
SUBF (R2),AC2 ;(1,1)
LDF 14(R2),AC0 ;(1,2)
DIVF AC2,AC0 ;AC0 ← (1,2) / [ 1 - (1,1) ]
LDF 10(R2),AC2 ;(3,1)
MULF AC0,AC2
ADDF 20(R2),AC2 ;(3,2)
MULF 4(R2),AC0 ;(2,1)
SUBF AC0,AC3
SUBF 20(R2),AC3 ;(2,2)
DIVF AC3,AC2 ;AC2 ← [(3,2)+(3,1)*(1,2)/[1-(1,1)] /
; [1-(2,2)-(2,1)*(1,2)/[1-(1,1)]
MULF AC5,AC2
STF AC2,AC4 ;AC4 ← Y
LDF 10(R2),AC2 ;(3,1)
MULF AC5,AC2 ;Z
4$: LDF 4(R2),AC3 ;(2,1)
MULF AC4,AC3 ;Y
ADDF AC2,AC3
LDF ONE,AC1
SUBF (R2),AC1 ;(1,1)
DIVF AC1,AC3 ;AC3 ← [(2,1)*Y+(3,1)*Z] / [1-(1,1)] = X
5$: LDF (SP)+,AC0 ;Retrieve the COS(angle)
RTS PC ; & Return to TAXIS or TMAGN
;Return vectors: SVMUL, VSDIV, TVMUL, VMAKE, VADD, VSUB
;83 -- 91 microseconds
SVMUL: ;Vector ← Scalar * Vector. Interpreter routine
;X ← S*X, Y ← S*Y, Z ← S*Z, W ← W
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector block]
MOV 2(R3),R2 ;R2 ← LOC[arg2] (the vector)
LDF @4(R3),AC0 ;AC0 ← arg1 (the scalar)
MOV #3,R1 ;R1 ← 3: How many fields to handle
1$: LDF (R2)+,AC1 ;AC1 ← next field of vector
MULF AC0,AC1 ;AC1 ← product
STF AC1,(R0)+ ;Store result
SOB R1,1$ ;Loop until all 3 fields done.
MOV (R2)+,(R0)+ ;Transfer W
MOV (R2)+,(R0)+ ; which is 2 words long.
MOV (R3)+,2(R3) ;Fix-up stack
TST (R3)+
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
VSDIV: ;Vector ← Vector / Scalar. Interpreter routine
;X ← X/S, Y ← Y/S, Z ← Z/S, W ← W
JSR PC,NOCMP ;Don't compact for a bit
LDF @(R3)+,AC0 ;AC0 ← arg2 (the scalar)
MOV (R3),R2 ;R2 ← LOC[arg1] (the vector)
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector block]
MOV #3,R1 ;R1 ← 3: How many fields to handle
1$: LDF (R2)+,AC1 ;AC1 ← next field of vector
DIVF AC0,AC1 ;AC1 ← product
STF AC1,(R0)+ ;Store result
SOB R1,1$ ;Loop until all 3 fields done.
MOV (R2)+,(R0)+ ;Transfer W
MOV (R2)+,(R0)+ ; which is 2 words long.
MOV (R3)+,(R3) ;Fix-up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
VMAKE: ;Interpreter routine
LDF @(R3)+,AC3 ;Fetch arg3 (Z)
LDF @(R3)+,AC2 ;Fetch arg2 (Y)
LDF @(R3)+,AC1 ;Fetch arg1 (X)
VMAKE0:: ;entry point for POINTY
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector]
STF AC1,(R0)+ ;Store X
STF AC2,(R0)+ ;Store Y
STF AC3,(R0)+ ;Store Z
MOV ONE,(R0)+ ;Store W
CLR (R0) ;Store W (second word)
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
VADD: ;Interpreter routine
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3)+,R0 ;R0 ← LOC[arg 2] (a vector)
MOV (R3)+,R1 ;R1 ← LOC[arg 1] (a vector)
LDF (R0)+,AC1 ;Calculate X
ADDF (R1)+,AC1
LDF (R0)+,AC2 ;Calculate Y
ADDF (R1)+,AC2
LDF (R0)+,AC3 ;Calculate Z
ADDF (R1)+,AC3
VRET: JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector]
STF AC1,(R0)+ ;Store X
STF AC2,(R0)+ ;Store Y
STF AC3,(R0)+ ;Store Z
MOV ONE,(R0)+ ;Assume W is 1
CLR (R0)
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
VSUB: ;Interpreter routine
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3)+,R1 ;R1 ← LOC[arg 2] (a vector)
MOV (R3)+,R0 ;R0 ← LOC[arg 1] (a vector)
LDF (R0)+,AC1 ;Calculate X
SUBF (R1)+,AC1
LDF (R0)+,AC2 ;Calculate Y
SUBF (R1)+,AC2
LDF (R0)+,AC3 ;Calculate Z
SUBF (R1)+,AC3
BR VRET ;Use common end code in VADD above
;283 -- 324 microseconds
TVMUL: ;Vector ← Trans * Vector. Interpreter routine
JSR PC,NOCMP ;Don't compact for a bit
MOV (R3),R2 ;R2 ← LOC[arg2] (the vector)
MOV 2(R3),R0 ;R0 ← LOC[arg1] (the trans)
CLRF AC1 ;X ← 0
CLRF AC2 ;Y ← 0
CLRF AC3 ;Z ← 0
MOV #4,R1 ;R1 ← How many columns left to go
1$: LDF (R2)+,AC0 ;AC0 ← field of vector
STF AC0,AC5 ;AC5 ← copy of AC0
MULF (R0)+,AC0
ADDF AC0,AC1 ;Add partial result to X
LDF AC5,AC0 ;Restore AC0
MULF (R0)+,AC0
ADDF AC0,AC2 ;Add partial result to Y
LDF AC5,AC0 ;Restore AC0
MULF (R0)+,AC0
ADDF AC0,AC3 ;Add partial result to Z.
SOB R1,1$ ;Go back to do all 4 columns.
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector]
STF AC1,(R0)+ ;Store X
STF AC2,(R0)+ ;Store Y
STF AC3,(R0)+ ;Store Z
MOV -4(R2),(R0)+;Copy W from the vector
MOV -2(R2),(R0) ; (2 words long)
MOV (R3)+,2(R3) ;Put result cell where first arg was
TST (R3)+ ; & fix up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
;Return a trans: TMAKE, TVADD, TVSUB, TTMUL, TINVRT, VSAXWR, CONSTR
TMAKE: ;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and the vector from the second part;
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans]
MOV 4(R3),R2 ;R2 ← LOC[arg 1] (the trans)
MOV #11,R1 ;R1 ← Count of how many copies to make
1$: MOV (R2)+,(R0)+ ;Transfer first half of floating word
MOV (R2)+,(R0)+ ;Transfer second half of floating word
SOB R1,1$ ;Repeat until done
MOV 2(R3),R2 ;R2 ← LOC[arg 2] (the vector)
MOV #3,R1 ;R1 ← Count of how many copies to make
2$: MOV (R2)+,(R0)+ ;Transfer first half of floating word
MOV (R2)+,(R0)+ ;Transfer second half of floating word
SOB R1,2$ ;Repeat until done
MOV (R3)+,2(R3) ;Fix-up stack
TST (R3)+
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code.
RTS PC ;Done.
TVCOM: ;Utility routine used to do common code in TVADD & TVSUB
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans]
MOV 2(R3),R2 ;R2 ← LOC[arg 2] (the vector)
MOV 4(R3),R1 ;R1 ← LOC[arg 1] (the trans)
MOV #11,R3 ;R3 ← Count of how many copies to make
1$: MOV (R1)+,(R0)+ ;Transfer first half of floating word
MOV (R1)+,(R0)+ ;Transfer second half of floating word
SOB R3,1$ ;Repeat until done
MOV #3,R3 ;R3 ← Count of how many additions to perform
RTS PC ;Return to TVADD or TVSUB
TVADD: ;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and add the vector from the first part to the second argument.
PUSH <R3> ;Save R3
JSR PC,TVCOM ;Do the common code for TVADD & TVSUB
1$: LDF (R1)+,AC0 ;AC0 ← word from trans
ADDF (R2)+,AC0 ; + word from vector
STF AC0,(R0)+ ;
SOB R3,1$ ;Repeat until done
TVRET: POP <R3> ;Restore R3
MOV -2(R3),2(R3) ;Fix-up stack (pretty strange huh?)
TST (R3)+
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code.
RTS PC ;Done.
TVSUB: ;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and subtract the second argument from the vector of the first arg.
PUSH <R3> ;Save R3
JSR PC,TVCOM ;Do the common code for TVADD & TVSUB
1$: LDF (R1)+,AC0 ;AC0 ← word from trans
SUBF (R2)+,AC0 ; + word from vector
STF AC0,(R0)+ ;
SOB R3,1$ ;Repeat until done
BR TVRET ;Do common end code & return
TTMUL: ;Interpreter routine
;Multiplies two transes together.
PUSH <R4> ;Save R4
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans]
MOV 2(R3),R2 ;R2 ← LOC[arg 2]
MOV 4(R3),R4 ;R4 ← LOC[arg 1]
PUSH <R3,R4> ;Save R3 & a copy of R4
MOV #4,R1 ;Loop count for cols of answer
1$: LDF (R2)+,AC1 ;Pick up a column of arg2: First row
LDF (R2)+,AC2 ; Second row
LDF (R2)+,AC3 ; Third row
STF AC3,AC4 ; store in AC4
MOV #3,R3 ;Loop count for rows of answer
2$: LDF (R4),AC3 ;First col of arg 1
MULF AC1,AC3
LDF 14(R4),AC0 ;Second col of arg 1
MULF AC2,AC0
ADDF AC0,AC3
LDF 30(R4),AC0 ;Third col of arg 1
MULF AC4,AC0 ;
ADDF AC0,AC3 ;
STF AC3,(R0)+ ;
ADD #4,R4 ;Move to the next column of arg 1
SOB R3,2$ ;Repeat for first 3 rows of answer
MOV (SP),R4 ;Reset R4 to point to first row of arg 1
SOB R1,1$ ;Repeat for all four columns of answer
LDF -14(R0),AC1 ;Add correction for last column, first row
ADDF 44(R4),AC1
STF AC1,-14(R0)
LDF -10(R0),AC1 ;Add correction for last column, second row
ADDF 50(R4),AC1
STF AC1,-10(R0)
LDF -4(R0),AC1 ;Add correction for last column, third row
ADDF 54(R4),AC1
STF AC1,-4(R0)
TST (SP)+ ;Pop the R4 temp
POP <R3,R4> ;Restore R3 & R4
MOV (R3)+,2(R3) ;Fix-up stack
TST (R3)+
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
TINVRT: ;Interpreter routine
COMMENT ⊗ Inverts a trans.
The result, (rot',trslat'), is defined:
rot' = transpose(rot)
trslat' = -(rot'*trslat)
⊗
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans] + 4*interation number
MOV 2(R3),R2 ;R2 ← LOC[old trans], travels down the whole trans
PUSH <R3,R4> ;Save R3 & R4
MOV R0,R3 ;R3 ← LOC[new trans] + 20*interation number
MOV R2,R4 ;R4 ← LOC[old trans], stays constant
MOV #3,R1 ;Three columns to do
1$: ;Transpose a column, multiplying by the translation
CLRF AC1 ;Cumulative product
LDF (R2)+,AC0 ;Take from the source rotation
STF AC0,(R0) ; into the transpose,
MULF 44(R4),AC0
SUBF AC0,AC1 ;accumulate the product.
LDF (R2)+,AC0 ;Take from the source rotation
STF AC0,14(R0) ; into the transpose,
MULF 50(R4),AC0
SUBF AC0,AC1 ;accumulate the product.
LDF (R2)+,AC0 ;Take from the source rotation
STF AC0,30(R0) ; into the transpose
MULF 54(R4),AC0
SUBF AC0,AC1 ;accumulate the product
STF AC1,44(R0) ;Place the new translation
ADD #4,R0 ;Move to next row of result
ADD #14,R3 ;Move to next column of result
SOB R1,1$
POP <R4,R3> ;Restore R4 & R3
MOV (R3)+,(R3) ;Fix-up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
VSAXWR: ;Interpreter Routine coded by ARG 5/3/76
;Produces a trans that rotates about a vector by a given angle
PUSH <R5> ;Save R5
LDF @(R3)+,AC2 ;Save angle in AC2
JSR PC,UNITV ;Convert vector to unit vector
STF AC2,AC0 ;Retrieve angle
JSR PC,@LSNCSD ;Get sin & cos of angle
STF AC0,AC4 ;Save sin in AC4
STF AC1,AC5 ;Save cos in AC5
SUBF ONE,AC1 ;AC1←(1-COS)
NEGF AC1
JSR PC,NOCMP ;Don't compact for a bit
JSR PC,GETTRN ;R0←-(R3)←LOC[New Tran]
MOV 2(R3),R1 ;R1←LOC[Unit Vec]
PUSH <#3> ;Three columns to do
1$: MOV #3,R5 ;Three rows to do
MOV 2(R3),R2 ;R2←LOC[Unit vec]
LDF AC1,AC2
MULF (R1)+,AC2 ;AC2←(1-COS)*U[i]
2$: LDF AC2,AC3
MULF (R2)+,AC3 ;Trans[j,i]←(1-COS)*U[i]*U[j]
STF AC3,(R0)+
SOB R5,2$ ;Do all 3 rows
DEC (SP)
BGT 1$ ;Do all 3 columns
POP <(R0)+>
CLR (R0)+ ;Set up last column
CLR (R0)+
CLR (R0)+
CLR (R0)+
CLR (R0)+
MOV #3,R5 ;Three terms to do: (1,1) (2,2) & (3,3)
MOV (R3),R0 ;R0←LOC[Trans]
3$: LDF AC5,AC1 ;AC1←COS
ADDF (R0),AC1 ;Add COS to (1-COS)*U[i]*U[i] term
STF AC1,(R0)
ADD #20,R0 ;R0 points to next term to add COS to
SOB R5,3$ ;Do all three terms
MOV (R3),R0 ;R0←LOC[Trans]
MOV 2(R3),R1 ;R1←LOC[Unit Vec]
LDF (R1)+,AC2 ;AC2←U[X]
MULF AC4,AC2 ;AC2←SIN*U[X]
STF AC2,AC3 ;Make a copy
ADDF 24(R0),AC2 ;Add it to the (3,2) term
STF AC2,24(R0)
NEGF AC3
ADDF 34(R0),AC3 ;Subtract it from the (2,3) term
STF AC3,34(R0)
LDF (R1)+,AC2 ;AC2←U[Y]
MULF AC4,AC2 ;AC2←SIN*U[Y]
STF AC2,AC3 ;Make a copy
ADDF 30(R0),AC2 ;Add it to the (1,3) term
STF AC2,30(R0)
NEGF AC3
ADDF 10(R0),AC3 ;Subtract it from the (3,1) term
STF AC3,10(R0)
LDF (R1)+,AC2 ;AC2←U[Z]
MULF AC4,AC2 ;AC2←SIN*U[Z]
STF AC2,AC3 ;Make a copy
ADDF 4(R0),AC2 ;Add it to the (2,1) term
STF AC2,4(R0)
NEGF AC3
ADDF 14(R0),AC3 ;Subtract it from the (1,2) term
STF AC3,14(R0) ;Trans is done!
POP <R5> ;Restore R5
MOV (R3)+,(R3) ;Clean up stack
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition codes
RTS PC ; & Return
CONSTR: ;Interpreter routine
JSR PC,NOCMP ;Don't compact for a while
MOV 4(R3),-(R3) ;Copy origin
JSR PC,VSUB ;Compute Vxy = V'xy - Vorg
MOV 2(R3),-(R3) ;Copy V'x
MOV 6(R3),-(R3) ;Copy origin
JSR PC,VSUB ;Compute Vx = V'x - Vorg
JSR PC,UNITV ;Make sure it's a unit vector
MOV (R3)+,2(R3) ;Keep a copy for later
MOV (R3),-(R3) ;Fix up stack for cross vector routine
MOV 4(R3),2(R3) ; with Vxy on top & Vx beneath it
JSR PC,CROSV ;Compute Vz = Vx ⊗ Vxy
JSR PC,UNITV ;Make it a unit vector
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[New trans]
MOV 4(R3),R1 ;R1 ← LOC[Vx]
MOV #6,R2
1$: MOV (R1)+,(R0)+ ;Copy Vx into first column of trans
SOB R2,1$
ADD #14,R0 ;R0 ← LOC[3rd column of trans]
MOV 2(R3),R1 ;R1 ← LOC[Vz]
MOV #6,R2
2$: MOV (R1)+,(R0)+ ;Copy Vz into third column of trans
SOB R2,2$
MOV 6(R3),R1 ;R1 ← LOC[Vorg]
MOV #6,R2
3$: MOV (R1)+,(R0)+ ;Copy Vorg into last column of trans
SOB R2,3$
MOV (R3)+,4(R3) ;Move LOC[trans] to bottom of stack
MOV (R3)+,R1 ;Fix up stack for cross vector routine
MOV (R3),-(R3) ; with Vx on top & Vz beneath it
MOV R1,2(R3)
JSR PC,CROSV ;Compute Vy = Vz ⊗ Vx
MOV (R3)+,R1 ;R1 ← LOC[Vy]
MOV (R3),R0 ;R0 ← LOC[trans]
ADD #14,R0 ;R0 ← LOC[2nd column of trans]
MOV #6,R2
4$: MOV (R1)+,(R0)+ ;Copy Vy into second column of trans
SOB R2,4$
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition codes
RTS PC ; & Return
;Affixment: AFFIX
AFFIX: ;Interpreter routine
COMMENT ⊗ This routine affixes two frames together. If necessary a frame header
will be created. An explicit trans may be given. The transes value is either
explicitly given on the stack, or created using the current values of the two
frames. Before doing the affixment a check is made that the frames are not
already affixed, and, if so, they are unfixed first. Frames which are affixed
to either arm are marked as being dynamic so they will be treated specially.⊗
FETCH R0 ;Get first frame offset
JSR PC,GETARG ;R0 ← LOC[environment entry]
BIT #HDRTYP,(R0) ;Test access type
BNE 1$
JSR PC,MFRAME ;If necessary make a new frame header
1$: MOV 2(R0),R2 ;R2 ← LOC[first frame header]
FETCH R0 ;Get second frame offset
JSR PC,GETARG ;R0 ← LOC[environment entry]
BIT #HDRTYP,(R0) ;Test access type
BNE 2$
JSR PC,MFRAME ;If necessary make a new frame header
2$: MOV 2(R0),R1 ;R1 ← LOC[second frame header]
AFFIX0: PUSH <R2,R1> ;Save LOC[frame headers]; entry point for pointy
JSR PC,UNFIX0 ;Unfix them if affixed & get their values
MOV #AFXSIZ,R0
JSR PC,GTFREE ;R0 ← LOC[new affixment node]
POP <R1,R2> ;Restore LOC[frame headers]
EVWAIT GNEVT ;Enter critical section
MOV CALCS(R2),(R0) ;Link into first frame's calculator list
MOV R0,CALCS(R2)
MOV R1,OTHER(R0) ;Point to who first is affixed to
MOV CALCS(R1),NEXT2(R0) ;Link into second frame's calculator list
MOV R0,CALCS(R1)
ADD #10,CALCS(R1) ;Make it point to the right place
MOV R2,US(R0) ;Point to who second is affixed to
EVSIG GNEVT ;End critical section
FETCH -(SP) ;Get the type info for the affixment
BIT #EXPTRN,(SP) ;Is an explicit trans specified?
BEQ 3$ ; nope
PUSH <R2,R1,R0> ; yes
FETCH R0 ;Get trans offset
JSR PC,GETARG ;R0 ← LOC[environment entry for trans]
TST (R0)+ ;R0 ← LOC[value pointer for trans]
POP <R1> ;R1 ← LOC[affixment node]
MOV R0,TRANS(R1) ;Fill in trans pointer
MOV R1,R0
POP <R1,R2> ;Restore regs
3$: TST (SP) ;See if trans value is already on stack
BPL 6$ ; yup
BIC #100000,(SP) ; nope - first clear implicit trans value bit
PUSH <R0,R2> ; & then calculate the trans
JSR PC,NOGC
CALL GETVAL,<R1> ;Get value of second frame
MOV R0,-(R3) ;Push value onto stack
BNE 4$
MOV #NILTRN,(R3) ;If none use NILTRAN as a default
4$: JSR PC,TINVRT
POP <R2>
CALL GETVAL,<R2> ;Get value of first frame
MOV R0,-(R3) ;Push value onto stack
BNE 5$
MOV #NILTRN,(R3) ;If none use NILTRAN as a default
5$: JSR PC,TTMUL ;Trans ← (first) * inv(second)
JSR PC,YESGC
POP <R0>
6$: BIS #AFXTYP,(SP) ;Mark the type as an affixment
MOV (SP),TYPE(R0) ;Set up type bits
MOV (SP),TYPE2(R0)
.IFNZ CPOINTY
MOV US(R0),R2 ;Make sure R2 points to first frame
MOV #-1,INVMRK(R2) ;invalidate first frame
.ENDC
BIS #FRAME2,TYPE2(R0)
BIT #EXPTRN,(SP)+ ;Now store away the trans value
BEQ 7$
MOV (R3)+,@TRANS(R0) ; use explicit trans
BR 8$
7$: MOV (R3)+,TRANS(R0) ; use implicit trans
8$: MOV US(R0),R1 ;Is either a device or affixed to a device?
MOV OTHER(R0),R2 ;R1 ← first frame, R2 ← second frame
BIT #FTYPE,TYPE(R1) ;See if first is a device
BNE 20$ ; No - try second
BIT #FTYPE,TYPE(R2) ;Second better not also be a device
BNE 21$ ;It's not - skip ahead and test if second is dynamic
BR 22$ ;Trying to affix two devices - go complain
20$: BIT #FTYPE,TYPE(R2) ;See if second is a device
BNE 24$ ;No - skip ahead
MOV R1,R0 ;Swap R1 & R2
MOV R2,R1
MOV R0,R2
21$: BIT #DYNAM,TYPE(R2) ;See if 2nd frame is already affixed to a device
BEQ 23$ ; No - skip ahead
CMPB MECH(R1),DCNT+1(R2) ;If so had better be this device
BEQ 30$ ;It is same device, so all done
22$: ALERR AFXERR ;It's a different device! Complain.
BR 30$ ; & then just punt
23$: MOV MECH(R1),R1 ;Get mech bits in R1
SWAB R1 ;R1 now has proper DCNT format: device,,cnt
BR 27$ ;Go mark second frame
24$: BIT #DYNAM,TYPE(R1) ;Neither frame was a device - see if 1st is dynamic
BNE 25$ ; Yes - skip ahead
BIT #DYNAM,TYPE(R2) ;See if second is dynamic
BEQ 30$ ; Nope - neither's dynamic so all done
MOV R1,R0 ;Swap R1 & R2
MOV R2,R1
MOV R0,R2
BR 26$ ;Go and mark first frame
25$: BIT #DYNAM,TYPE(R2) ;See if 2nd frame is already affixed to a device
BEQ 26$ ; No - skip ahead and mark it
CMPB DCNT+1(R1),DCNT+1(R2) ;If so had better be this device
BEQ 30$ ;It is same device, so all done
BR 22$ ;It's not the same device - go complain
26$: MOV DCNT(R1),R1 ;Get DCNT in R1
27$: INC R1 ; & update the count
JSR PC,AFXAUX ;Finally, mark the frame as dynamic
30$: CCC
RTS PC ;Done
AFXAUX: ;Auxillary routine used by AFFIX to run down the affixment chain of
;a frame that has just become dynamic. R2 points to the frame, R1 has
;the dev,,cnt info, R0 is used to run through the list.
BIT #FTYPE,TYPE(R2) ;See that we've got a frame
BNE 1$ ; Yup - skip ahead
MOV MECH(R2),R1 ;Get mech bits in R1
SWAB R1 ;R1 now has proper DCNT format: device,,cnt
BR 2$ ;Go mark other frames
1$: BIT #DYNAM,TYPE(R2) ;See if we've been marked as dynamic yet
BNE 4$ ; Yup - all done here
BIS #DYNAM,TYPE(R2) ;Mark us as being dynamic
MOV R1,DCNT(R2) ;Indicate device we're affixed to & counter
2$: INC R1 ;Increment depth counter
MOV CALCS(R2),R0 ;R0 points to affixment chain to try marking
BEQ 4$ ; if any
3$: PUSH <R1,R0> ;Save regs
MOV OTHER(R0),R2 ;R2 points to frame we're affixed to
JSR PC,AFXAUX ;Try to mark him
POP <R0,R1>
MOV (R0),R0 ;Run down affixment list
BNE 3$
4$: RTS PC ;All done
DATA
AFXERR::ASCIE </Can't have affixment chain connecting two arms!/>
CODE
; UNFIX
UNFIX: ;Interpreter routine
COMMENT ⊗ This routine unfixes two frames. Before unfixing, an attempt is made
to validate both frames. ⊗
FETCH R0 ;Get first frame offset
JSR PC,GETARG ;R0 ← LOC[environment entry]
BIT #HDRTYP,(R0) ;Check header exists
BEQ UNRET ; if not quit
MOV 2(R0),R2 ;R2 ← LOC[first frame header]
FETCH R0 ;Get second frame offset
JSR PC,GETARG ;R0 ← LOC[environment entry]
BIT #HDRTYP,(R0) ;Check header exists
BEQ UNRET ; if not quit
MOV 2(R0),R1 ;R1 ← LOC[second frame header]
UNFIX0: ;Entry point from AFFIX
PUSH <R1,R2>
BIT #FTYPE,TYPE(R1) ;Try to validate both frames before we unfix them
BEQ 10$ ; Unless they're devices
CALL GETVAL,<R1>
10$: MOV (SP),R1
BIT #FTYPE,TYPE(R1)
BEQ 11$
CALL GETVAL,<R1>
11$: MOV (SP),R2 ;Restore R2 & R1, but leave pointers on stack
MOV 2(SP),R1
EVWAIT GNEVT ;Enter critical region
ADD #CALCS,R1 ;R1 ← LOC[beginning of second's calculator list]
1$: MOV (R1),R0 ;R0 ← LOC[next calc to check]
BEQ 2$ ; if any
BIT #AFXTYP,TYPE(R0) ;Make sure it's an affixment
BEQ 2$
CMP R2,OTHER(R0) ;See if affixed to first frame
BEQ 3$ ; yes - found it
2$: MOV (R1),R1 ;Check next
BNE 1$ ; if any
CMP (SP)+,(SP)+ ;Clear R1 & R2 off of stack
BR 30$ ;Whoops - wasn't there so split
3$: MOV (R0),(R1) ;Remove us from second's calc list
BIT #FRAME2,TYPE(R0)
BNE 4$ ;Treat the second frame slightly differently
PUSH <R0> ;Save LOC[affixment node]
ADD #10,R0 ;R0 ← LOC[node as seen by other frame]
BR 5$
4$: SUB #10,R0
PUSH <R0>
5$: ADD #CALCS,R2 ;R2 ← LOC[beginning of first's calculator list]
6$: CMP (R2),R0 ;Find affixment node in the list
BEQ 7$
MOV (R2),R2 ;Check next node
BNE 6$
BR 8$ ;Wasn't there!
7$: MOV (R0),(R2) ;Unlink node from list
8$: POP <R0> ;R0 ← LOC[affixment node]
JSR PC,RLFREE ;Release it
POP <R2,R1> ;Restore pointers to frame headers
BIT #FTYPE,TYPE(R1) ;Is first a device?
BEQ 21$ ; Yes - unmark second
BIT #FTYPE,TYPE(R2) ;Is second a device?
BEQ 20$ ; Yes - go unmark first frame
BIT #DYNAM,TYPE(R1) ;Is first (and hence second) dynamic?
BEQ 30$ ; Nope - nothing to do so all done
CMP DCNT(R1),DCNT(R2) ;See who's further away from device
BEQ 30$ ;If they're the same distance do nothing
BLT 21$ ;First is closer unmark second
20$: MOV R1,R2 ;Second is closer - unmark first frame
21$: MOV DCNT(R2),R1 ;DCNT to use to detect loops in affixment chain
JSR PC,UFXAUX ;Go unmark frame pointed to by R2
30$: EVSIG GNEVT ;End critical section
UNRET: CCC
RTS PC ;Done
UFXAUX: ;Auxillary routine used by UNFIX to run down the affixment chain of
;a frame that has just ceased being dynamic. R2 points to the frame,
;R1 has the dev,,cnt info, R0 is used to run through the list.
BIT #FTYPE,TYPE(R2) ;Check that we've got a frame
BEQ 1$ ; No, it's a device! Need to remark.
BIT #DYNAM,TYPE(R2) ;See if we've been unmarked as dynamic yet
BEQ 4$ ; Yup - all done here
BIC #DYNAM,TYPE(R2) ;Mark us as no longer being dynamic
CMP R1,DCNT(R2) ;Check that there isn't a loop in the affixment
BLE 2$ ; No loop finish unmarking frame
MOV DCNT(R2),R1 ; Yes there is a loop! Use this DCNT
1$: JSR PC,AFXAUX ; & Remark everyone
CLR R1 ;Indicate to whoever called us that we're finished
BR 4$
2$: CLR DCNT(R2) ;DCNT no longer counts
MOV CALCS(R2),R0 ;R0 points to affixment chain to try marking
BEQ 4$ ; if any
3$: PUSH <R0> ;Save reg
MOV OTHER(R0),R2 ;R2 points to frame we're affixed to
JSR PC,UFXAUX ;Try to unmark him
POP <R0>
TST R1 ;See if DCNT is 0, means loop in affixment found
BEQ 4$ ; Yes - all done then
MOV (R0),R0 ;Run down affixment list
BNE 2$
4$: RTS PC ;All done
;Motion: MOVE, CENTER, OPERATE, STOP
MOVE: ;Interpreter routine
MOV LMOVE,R2 ;Set for moving operation
JMP MOVSTA ;Use the common move code
CENTER: ;Interpreter routine
MOV LCENTER,R2 ;Set for centering operation
JMP MOVSTA ;Use the common move code
OPERATE:;Interpreter routine
MOV LOPERATE,R2 ;Set for device operation
MOV #'π,R0 ;Whistle while you work
JSR PC,TYPCHR
MOV #12,R0 ;Get a block for the coefficient list
JSR PC,GTFREE
PUSH <R0> ;Save a copy on the stack
FETCH (R0)+ ;Store in servo bits
CLR (R0)+ ;Clear 2nd word of servo bits
FETCH (R0)+ ;Store in command bits
CLR (R0)+ ;Clear wobble ptr
CLR (R0)+ ;Clear rel seg ptr
LDF @(R3)+,AC0 ;AC0 ← time allocated for operation
ABSF AC0 ;Make sure time is positive
MULF THOUS,AC0 ;AC0 ← time, in milliseconds
STCFI AC0,(R0)+ ;Store away time
LDF @(R3)+,AC0 ;AC0 ← vise stop-wait time or driver torque
LDF @(R3)+,AC1 ;AC1 ← vise position or driver velocity
BIT #VISESB,@(SP) ;Check whether vise or driver
BEQ 1$ ;Handle driver below
MULF THOUS,AC0 ;Convert stop-wait time to milliseconds
BR 2$
1$: MULF THOUS,AC1 ;Convert driver velocity to degrees/millisecond
2$: STF AC0,(R0)+ ;Store them away
STF AC1,(R0)+
MOV #DVBKSZ,R0 ;Get a device block
JSR PC,GTFREE
MOV R0,R1 ;R1 ← address of device block
MOV (SP)+,R0 ;R0 ← coefficient list
JMP OPMOV ;Use the common move code
STOP: ;Interpreter routine
COMMENT ⊗ Takes one argument, a set of mechanism bits. (e.g. BARM,
BHAND, YARM, YHAND). For each one on, all the associated joints are
stopped. ⊗
FETCH R2 ;R2 ← mechanism bits
MOV R2,R0 ;R0 ← mech bits
JSR PC,TABOFS ;R0 ← table offset
BIT #ANARM,R2 ;An arm?
BEQ 1$ ;No
MOV #6,R1 ;R1 ← count of joints
BR 2$
1$: MOV #1,R1 ;R1 ← count of joints
2$: ADD LDVCPTR,R0 ;R0 ← LOC[table of device pointers]
3$: MOV (R0)+,R2 ;R2 ← device block
BEQ 4$ ;If any
TST (R2) ;Make sure still valid
BEQ 4$
BIS #100000,@0(R2) ;Stop this device.
4$: SOB R1,3$ ;Repeat
SLEEP #200. ;Sleep for 200 milliseconds so arm can stop
CCC ;Clear condition code
RTS PC ;Done
;Common code for motions: MOVSTA & OPMOV
DVBKSZ == 24 ;Size of a device block (2+#jts)
COMMENT ⊗ New version to update the frame afterwords. Assumes that
there are two arguments: a pointer to the trajectory table and a word
of mechanism bits, to help in updating the necessary variables. ⊗
MOVSTA: MOV #'π,R0 ;Whistle while you work
JSR PC,TYPCHR
MOV #DVBKSZ,R0 ;Get a device block
JSR PC,GTFREE
MOV R0,R1 ;R1 ← address of device block
FETCH R0 ;R0 ← address of coefficient list
OPMOV: PUSH <R1,R0> ;Save pointers so we can reclaim these later
41$: CMPB SAILID,#2 ;See if we're talking to the 10
BNE 42$ ;Nope - skip ahead
TST NOTB10 ;Gathered force data still there?
BEQ 42$ ;Nope - skip ahead
TST NOTB10+2 ;Check data valid flag - if still set wait
BEQ 42$ ;Nope - skip ahead
SLEEP #144 ;No - wait 100 msecs then try again
BR 41$
42$: JSR PC,NOCMP ;Don't compact for a bit
.IFNZ CPOINTY
MOV #1,DSPOK ; kill the display for a while
.ENDC
JSR PC,@R2 ;Do some kind of move (MOVE, CENTER, OPERATE)
.IFNZ CPOINTY
CLR DSPOK ; turn on the display again
.ENDC
LDCIF R0,AC0 ;Convert error bits to scalar
PUSH <R0> ;Save them too
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar]
STF AC0,(R0)
CMPB SAILID,#2 ;See if we're talking to the 10
BNE 40$ ;Nope - skip ahead
BIT #ANARM,@(R4);See if we just moved an arm - IPC points to mech bits
BEQ 40$ ;Nope - skip ahead
TST NOTB10 ;Any gathered force data to send over?
BEQ 40$ ;Nope - skip ahead
TST NOTB10+2 ;Check data valid flag - shouldn't be set yet
BNE 40$ ;Already have signalled 10
INC NOTB10+2 ;Set data valid
MOV #1,172566 ;Wake up pdp10 by generating interrupt
CMP FPTR,#PCODE ;Check if force data has overwritten pcode
BLO 40$ ;Okay - skip ahead
ALERR GATERR ;Complain
;Invalidate the affected device variables & update error variables
40$: FETCH R2 ;R2 ← mechanism bits
BIT #YARM,R2
BEQ 1$
MOV #YARMHD,R0 ;Header for YARM
JSR PC,INVLDT
MOV (R3),YAERR+2
1$: BIT #YHAND,R2
BEQ 2$
MOV #YHANDH,R0 ;Header for YHAND
JSR PC,INVLDT
MOV (R3),YHERR+2
2$: BIT #BARM,R2
BEQ 3$
MOV #BARMHD,R0 ;Header for BARM
JSR PC,INVLDT
MOV (R3),BAERR+2
3$: BIT #BHAND,R2
BEQ 4$
MOV #BHANDH,R0 ;Header for BHAND
JSR PC,INVLDT
MOV (R3),BHERR+2
4$: BIT #DRIVER,R2
BEQ 5$
MOV (R3),DRERR+2 ;Nothing to invalidate
5$: BIT #VISE,R2
BEQ 7$
CALL GETVAL,<#FJAWH> ;Try to get a value for the fixed jaw
TST R0 ;Did we suceed?
BEQ 6$ ;Nope
MOV #MJAWH,R0 ;Header for MOVING-JAW
JSR PC,INVLDT
CLR FJAWH+INVMRK ;Mark FIXED-JAW as still valid
6$: MOV #VISEH,R2
JSR PC,GETDEV ;Get current vise opening
LDF @(R3)+,AC0 ;Put it in AC0
MOV VISOP+2,R0 ;R0 ← LOC[vise opening trans]
STF AC0,50(R0) ;Update y value in trans
MOV (R3),VIERR+2
7$: TST (R3)+ ;Flush device error bits
JSR PC,YESCMP ;OK to compact now
POP <R2,R0> ;R2 ← error bits, R0 ← coefficient list
BIT #VISESB+DRVRSB,(R0) ;See if device coefficient list
BEQ 8$
JSR PC,RLFREE ;If so get rid of the it
8$: BIT #135400,R2 ;Associated joint #?
BEQ 11$ ; No - skip ahead
MOV (SP),R1 ;Get address of device block
MOV (R1)+,R0 ;Maximum number of joints in device block
TST (R1)+ ;Point to first joint
9$: BIT #177400,(R1)+ ;Is this the offending joint?
BNE 10$ ; Yup - found it
SOB R0,9$ ;Try next joint
BR 11$
10$: MOV -(R1),R2 ;Change R2 so the low 2 digits give joint #
11$: POP <R0>
JSR PC,RLFREE ;Get rid of the device block
FETCH R0 ;Get error bit mask
TST R2 ;Any errors?
BEQ 30$ ;Nope - all done
BIT #177400,R2 ;Any high byte bits on?
BEQ 20$ ;No, handle low byte errors ourselves
MOV R2,R1
BIC #177,R1 ;R1 ← error bits - jt #
BIC R0,R1 ;Is this what the poor user wants to handle himself?
BNE 20$ ;No - handle it below
BMPIPC ;Bump IPC past address of next pcode
BMPIPC ;Bump IPC past retry address
CCC
RTS PC ;Go do the user's error handling code
20$: MOV R2,R0
PUSH <R0> ; save error code
EVWAIT CSLEVT ;Grab the console
CMP R0,#7 ;Power supply off?
BNE 21$
MOV MOVERS,R0 ;Yup - R0 ← address of error message
BR 22$
21$: CMP R0,#16 ;Background force job over run?
BNE 23$
MOV MOVERS+2,R0 ;Yup - R0 ← address of error message
22$: JSR PC,TYPSTR ; Complain
POP <R0>
BR 26$
23$: MOV #2000,R1 ;Check for: Panic button pushed, Excessive force, Time out,
; Stop limit exceeded & No arm solution while force servoing
MOV #MOVERS+4,R2;Pointer to list of error message addresses
24$: BIT R0,R1 ;Is this the error?
BNE 25$ ;Yes - found it
TST (R2)+ ;Advance error message pointer
ASL R1 ;Try next servo error
BPL 24$ ;Go see if this is the one - unless we've checked them all
25$: MOV (R2),R0 ;R0 ← address of error message
JSR PC,TYPSTR ; Complain
POP <R0>
BIC #76000,R0 ;(exclude ex.force,timeout,joint lim,panic but & no arm sol)
BEQ 26$
JSR PC,TYPOCT ; Give error condition
26$: EVSIG CSLEVT ;Release the console
ALERR MOVERR ; and switch to DDT
30$: JMP JUMP ;Jump to next pcode address
DATA
MOVERS: .WORD 1$,2$,3$,4$,5$,6$,7$,8$ ;Pointers to error messages
1$: .ASCIZ /
ARM INTERFACE POWER SUPPLY TURNED OFF
(CHECK JOINT BRAKE SWITCHES)/ ; 7
2$: .ASCIZ /
BACKGROUND JOB TOOK TO LONG TO EXECUTE / ;16
3$: .ASCIZ /
PANIC BUTTON PUSHED/ ; 2000
4$: .ASCIZ /
EXCESSIVE FORCE ENCOUNTERED BY JOINT / ; 4000
5$: .ASCIZ /
TIME OUT FOR JOINT / ; 10000
6$: .ASCIZ /
STOP LIMIT EXCEEDED FOR JOINT / ; 20000
7$: .ASCIZ /
NO ARM SOLUTION WHILE DOING FORCE COMPLIANCE / ; 40000
8$: .ASCIZ /
SERVO ERROR = /
MOVERR::ASCIE </TO RETRY THE MOVE, RETRY$G/>
GATERR::ASCIE </PCODE overwritten by gathered force data!/>
CODE
;Error recovery for motions: RETRY, FINISH, PARK
RETRY: TST (SP)+ ;Get here from ALERR; clean off stack
.IFNZ CPOINTY
; MOV #1,RPFLAG ;Tell POINTY that this is a RETRY
.ENDC
RETRY1: BMPIPC ;Bump IPC - to retry address
JMP JUMP ;Try the whole move again - from the top
FINISH: TST (SP)+ ;Get here from ALERR; clean off stack
MOV (R4),R0 ;R0 ← IPC
BIT -4(R0),#ANARM ;Make sure it's an arm
BEQ RETRY1 ;Otherwise just do a RETRY
MOV -4(R0),DUMMOV+4 ;Set up proper mech bits
MOV -6(R0),R0 ;R0 ← LOC[trajectory]
MOV (R0),TRAJ ;Use requested arm
ADD #10,R0 ;R0 ← LOC[REL SEG POINTER]
1$: MOV R0,R1
ADD (R1),R0 ;R0 ← next motion segment
TST (R0) ;If any
BNE 1$ ;Keep at it until we find last segment
MOV 4(R1),TRAJTP;Use trans pointer from this segment
BNE 3$ ;It has one so go on ahead
MOV #TRAJCF,R0 ;Else we need to copy the coefficients
ADD #10,R1
MOV #6*6,R2 ;Arms have 6 joints, each with 6 terms
2$: LDF (R1)+,AC0 ;So copy it
STF AC0,(R0)+
SOB R2,2$
3$: JSR PC,FAKMOV ;Fake the move
JMP JUMP ;Proceed with the program
FAKMOV: PUSH (R4) ;Save IPC
1$: MOV #DUMMOV+2,(R4) ;IPC of dummy move
JSR PC,2$ ;Do it - indirect so RETRY will work
CMP #DUMMOV,(R4) ;See if attempting a RETRY
BEQ 1$ ;Yup - so try it all again
POP (R4) ;Restore old IPC
RTS PC
2$: JSR PC,MOVE ;If the move succeeds we'll return here
RTS PC ; otherwise RETRY will return us
PARK: MOV #BAOFST,R0
JSR PC,GVAL0 ;Get current blue arm position
MOV (R3),R2
LDF BPARK+44.,AC0 ;Copy bpark height
STF AC0,44.(R2)
MOV (R3)+,FINITR+2 ;Now finish trans = height of 9in above current position
MOV #TPNTR,TRAJTP ; & the move will use the finish trans
MOV #770,TRAJ ;Use Blue arm
MOV #4,DUMMOV+4 ;Set up proper mech bits
MOV #2*6*6,R0 ;Arms have 6 joints, each with 6 fp terms
MOV #TRAJCF,R2
1$: CLR (R2)+ ;Zero out traj coeffs
SOB R0,1$
JSR PC,FAKMOV ;Fake the move - first move up
MOV #BPARK,FINITR+2 ;Now finish trans = bpark
JSR PC,FAKMOV ;Fake the move - now go to bpark
;Eventually we'll want to repeat this for the yellow arm
2$: BPT ;Don't let user proceed beyond here
BR 2$
DATA
3
BPARK: .FLT2 -1.0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 1.0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 -1.0000000
.FLT2 43.5312500
.FLT2 56.8550000
.FLT2 9.9587500
DUMMOV: XMOVE ;Pcode for fake motion
TRAJ
4
0
0
DUMMOV
;Trans pointer for motion
TPNTR: FINIOF
0
;Motion table
TRAJ: 770
0
1
0
;Relative segment pointer
310
4000. ;Time for motion in msec
TRAJTP: TPNTR ;Trans pointer
0
;Coefficients, gravity, inertia
TRAJCF: .FLT2 180.0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 -90.0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 14.0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 -90.0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 90.0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 .0000000
.FLT2 225498800.0000000
.FLT2 297.5199900
.FLT2 452568800.0000000
.FLT2 .0000000
.FLT2 535300.0100000
.FLT2 .0000000
.FLT2 12673000.0000000
.FLT2 .0000000
.FLT2 12470000.0000000
.FLT2 .0000000
.FLT2 887399.9900000
0
;End of motion table
CODE
;Force system routines: SETBAS, WRIST, STIFF, GATHR
;Interpreter routine
SETBAS: CLR R0 ;Don't return the matrix
JSR PC,@LSETBAS ;Go calibrate the wrist
CCC
RTS PC ;All done
WRIST: MOV #6*2,R0 ;Get enough room to store 6 floating point force values
;Interpreter routine
JSR PC,GTFREE
MOV R0,R1 ;R1 ← address of device block
PUSH <R0> ;Save a copy on the stack
CLR R0 ;Use internal calibration matrix
JSR PC,@LWRIST ;Go read the wrist
FETCH R0 ;R0 ← offset for variable to store force vector in
JSR PC,GETARG ;R0 ← LOC[env entry for force vector:K]
PUSH <R0> ;Save it
FETCH R0 ;R0 ← offset for variable to store torque vector in
JSR PC,GETARG ;R0 ← LOC[env entry for torque vector:G]
PUSH <R0> ;Save this one too
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector]
JSR PC,GETVEC ;R0 ← -(R3) ← LOC[new vector] - Get two of them
POP <R0,R1> ;R0 ← G, R1 ← K
MOV (R3),2(R1) ;Store pointer to force vector away in environment
MOV 2(R3),2(R0) ; ditto for torque vector
MOV (SP),R2 ;R2 ← LOC[force components]
MOV #2,R0 ;# of vectors to transfer
1$: MOV (R3)+,R1 ;R1 ← LOC[force/torque vector]
LDF (R2)+,AC0 ;Get 1st force component
STF AC0,(R1)+ ;Store it in vector
LDF (R2)+,AC0 ; ditto for 2nd component
STF AC0,(R1)+
LDF (R2)+,AC0 ; & likewise for 3rd component
STF AC0,(R1)+
SOB R0,1$ ;Do both vectors
POP <R0> ;R0 ← LOC[force component block]
JSR PC,RLFREE ;Release it
CCC
RTS PC ;All done
STIFF: MOV #14,R0 ;Get a block to store the 6 stiffness values into
;Interpreter routine
JSR PC,GTFREE ;R0 ← address of block
PUSH <R0> ;Save a copy on the stack
MOV #2,R2
1$: MOV (R3)+,R1 ;R1 ← LOC[force/torque vector]
LDF (R1)+,AC0 ;Pick up stiffness value
STF AC0,(R0)+ ;Stuff it into block
LDF (R1)+,AC0 ;do it again
STF AC0,(R0)+
LDF (R1)+,AC0 ;once more
STF AC0,(R0)+
SOB R2,1$
FETCH R0 ;R0 ← c-sys bits (ignored now)
MOV (SP),R0 ;R0 ← LOC[stiffness value block]
MOV (R3)+,R1 ;R3 ← LOC[compliance center]
JSR PC,@LSETSTF ;Call set stiffness routine
POP <R0>
JSR PC,RLFREE ;Release stiffness value block
RTS PC ;All done
GATHR: ;Interpreter routine
FETCH R0 ;Get control bits
1$: CMPB SAILID,#2 ;See if someone on the 10 is there to talk with us
BNE 3$
TST NOTB10 ;Data from last GATHER read yet?
BEQ 2$ ;Yup - skip ahead
SLEEP #144 ;No - wait 100 msecs then try again
BR 1$
2$: MOV #IPTR,R2 ;Pointer for ID #, force bits & # pts - used by 10 program
MOV #DATEND,(R2)
MOV #FPTR,R1 ;Pointer to where to store force readings
MOV #DATEND+6,(R1)
MOV #DATEND,NOTB10 ;Set up pointer to data buffer for 10
CLR NOTB10+2 ;Clear data valid flag
PUSH R3 ;Save R3 stack
INC GCNT ;Update GATHER counter
MOV GCNT,R3
JSR PC,@LGATHER ;Go tell force system we want force data gathered
POP R3 ;Restore R3 stack
3$: CCC
RTS PC ;All done
DATA
GCNT: 0 ;Use a unique number for each gather
IPTR: DATEND ;Pointer for ID #, force bits, & # pts
FPTR: DATEND+6 ;Pointer for force readings
CODE
;Motion auxilary functions: TABOFS, WHERE, NOTICE
COMMENT ⊗ Certain tables are available via COMTAB entries. LERRPTR
points to the table ERRPTR of 16 words, one for each servo, which
points at the current error torques. LTHPTR points at the table THPTR
of 16 words, one for each servo, which points at the current joint
angles. ⊗
TABOFS:
COMMENT ⊗ R0 = Mechanism bit. Returns table offset (in bytes) in R0.
For example, if the mechanism is BARM, the OFBARM is returned. ⊗
MOV #OFTAB,R1 ;R1 ← start of mech/offset table
1$: BIT (R1)+,R0 ;Is it this mechanism?
BEQ 2$ ;No - skip ahead
MOV (R1),R0 ;Yes - Load up proper offset from table
RTS PC ; and return.
2$: TST (R1)+ ;Advance to next table entry
TST (R1) ;Check if at end of table
BNE 1$ ; & if more check them
ALERR TABMES ;Illegal
CLR R0
RTS PC
DATA
OFTAB: .WORD YARM, OFYARM, YHAND, OFYHAND, BARM, OFBARM, BHAND, OFBHAND
.WORD VISE, OFVISE, DRIVER, OFDRIVER, 0
TABMES::ASCIE </ILLEGAL MECHANISM/>
CODE
WHERE: ;Interpreter routine
COMMENT ⊗ One argument: The mechanism bits. Puts value of that
mechanism on the stack. Only one mechanism at a time, please! ⊗
FETCH R2 ;Mechanism bits
JSR PC,NOCMP ;Don't compact for a bit
BIT #ANARM,R2 ;An arm?
BEQ 1$ ;No - skip
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans]
BR 2$
1$: JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar]
2$: MOV LTHPTR,R1 ;
JSR PC,@LUPDATE ;
JSR PC,YESCMP ;OK to compact now
CCC ;Clear condition code
RTS PC ;Done
NOTICE:
COMMENT ⊗ The arms may have been moved without our knowledge. A call
to this routine invalidates all manipulator variables. This routine
should be called from WAITE, but not from MOVE or CENTER. It may be
called from DDT, since it saves all registers. ⊗
PUSH <R0,R1,R2>
;Invalidate all manipulator variables
MOV #BHANDH,R0
JSR PC,INVLDT
MOV #BARMHD,R0
JSR PC,INVLDT
MOV #YHANDH,R0
JSR PC,INVLDT
MOV #YARMHD,R0
JSR PC,INVLDT
COMMENT ⊗ Leave this out until Ken brings the vise up.
CALL GETVAL,<#FJAWH> ;Try to get a value for the fixed jaw
TST R0 ;Did we suceed?
BEQ 1$ ;Nope
MOV #MJAWH,R0 ;Header for MOVING-JAW
JSR PC,INVLDT
CLR FJAWH+INVMRK ;Mark FIXED-JAW as still valid
1$: MOV #VISEH,R2
JSR PC,GETDEV ;Get current vise opening
LDF @(R3)+,AC0 ;Put it in AC0
MOV VISOP+2,R0 ;R0 ← LOC[vise opening trans]
STF AC0,50(R0) ;Update y value in trans
⊗
POP <R2,R1,R0>
RTS PC
;Condition monitors: CMMAK
COMMENT ⊗ This is the third version of condition monitors: modified by arg 5/77
(here refered to as c-m's). Hardware-type c-m's will be ready soon. (hah! 1/78)
The basic operations are Creation, Enabling, Disabling, Destruction.
Creation causes a c-m control block to be set up, and pointed to by
the c-m variable. This block has the following fields: ⊗
II == 0
XX CMTYPE ;Type of c-m: event,expression,duration,force or hardware
CMEVT == 0 ;Event type c-m
CMEXP == 1 ;Expression type c-m
CMDRA == 2 ;Duration type c-m
CMFRC == 3 ;Force sensing type c-m
CMHRD == 4 ;Hardware monitor type c-m
XX CMISB ;LOC[ISB] of the c-m
XX CMSTRT ;Starting address of c-m
XX CMBITS ;Bits needed for: force & hardware c-m's
XX CMSTAT ;Status bits for the c-m
CMENB == 1 ;set => enabled
CMDES == 2 ;set => to be destroyed
CMRUN == 4 ;set => c-m is currently running
CMCBSZ == II/2 ;Length in words of a c-m control block.
II == 4 ;for event & expression c-m's
XX CMSEVT ;The event used to awaken the tester upon enabling
COMMENT ⊗ The various types of condition monitors are each handled
differently. Basically each c-m is an independent process which runs
in parallel with the process that creates it. Each c-m is an interpreter
and runs at priority 2 (exception: the checker part of an expression c-m
runs at priority 3). When a c-m is created by CMMAK, new PDB, ISB and
CMCB blocks are made. For duration, force and hardware c-m's nothing
further is done until they are enabled or destroyed. Enabling causes
the c-m checker part to be interpreted and to place the c-m body in the
appropriate queue, so it will be run if & when the condition being
checked for occurs. Disabling removes the c-m from the queue. Destroying
the c-m causes it to be disabled and then it's PDB, ISB & CMCB are all
reclaimed. At the conclusion of the body if the c-m has been re-enabled
it reschedules itself in the appropriate queue and then dismisses.
Event and expression c-m's, after initialization, wait for the
gronking event CMSEVT. Enabling signals the event CMSEVT and sets
the enabled bit in CMSTAT. Disabling resets the enabled bit, and the
c-m will wait on the CMSEVT for future action. As long as the c-m is
enabled, it periodically wakes up, checks its status bits. If the
enable bit is reset, the c-m waits for CMSEVT. Else it checks the
condition. If it is satisfied, the c-m disables itself and
proceeds to the conclusion (at level 2, the conclusion should reset
itself to level 1 after all critical activity has been accomplished,
although this is not currently done.) Otherwise, it reschedules itself.
If the destroy bit should ever be set in CMSTAT, then the c-m will
destroy the event CMSEVT. Then it will reclaim the c-m control blocked
and will dismiss, never to return. (The pointer to the c-m in the
environment should be zeroed by the destroying angel.). ⊗
CMMAK: ;Auxillary routine
COMMENT ⊗ Takes three (or four) arguments: the type of the nascent c-m, the
IPC of the c-m code, the environment size needed, & optionally the level-offset
of the event that this monitor is to wait on, or the bits needed to specify
force sensing. Called with the number of cmons to make in R0 and R2 pointing
at the environment entry. ⊗
1$: PUSH <R0> ;Save count of how many cmons to make
;Make a c-m control block
MOV #CMCBSZ,R0
JSR PC,GTFREE ;R0 ← LOC[c-m control block]
MOV #CMNTYP,(R2)+ ;Set data type to cmon
MOV R0,(R2)+ ;Stuff into environment
PUSH <R2> ;Save environment pointer
FETCH CMTYPE(R0) ;Get type of c-m
PUSH <R0> ;Save LOC[c-m control block]
;Prepare the c-m job
FETCH CMSTRT(R0) ;Store away IPC of start of c-m code
MOV CMSTRT(R0),R0 ;R0 ← IPC of c-m code
CLR R1 ;C-m's do not expire with events
JSR PC,SPAWN ;R0 ← process control block for c-m
MOV (SP),R1 ;R1 ← LOC[CMCB]
MOV PDBR4(R0),R2 ;R2 ← PR4 = LOC[c-m's interpeter status block]
MOV R2,CMISB(R1) ;Store away location of c-m's ISB
MOV R1,CMCB(R2) ;Stuff CMCB of the c-m
MOV #UGRSAV+UFPUSE+4,PDBSTA(R0) ;c-m's run with priority = 2
MOV #144040,UPSW(R0)
CMP #CMEXP,CMTYPE(R1) ;If expression c-m runs with priority = 3
BNE 2$
MOV #UGRSAV+UFPUSE+6,PDBSTA(R0) ;Change priority to 3
MOV #144140,UPSW(R0)
2$: MOV R0,R2 ;R2 ← new process descriptor block
;Set up the new environment
JSR PC,NEWENV ;R0 ← LOC[new environment]
MOV ENV(R4),SLINK(R0) ;Not necessary to set up OLEV, etc.
MOV PDBR4(R2),R1
MOV R0,ENV(R1)
INC LEV(R1)
POP <R0> ;R0 ← LOC[CMCB]
CMP #CMEXP,CMTYPE(R0) ;See what type of c-m we've got
BLT 4$ ;Duration, force sensing or hardware - jump ahead
EVMAK ;Expression or Event cmons
POP <CMSEVT(R0)> ;Make an event for CMSEVT
FORK R2,#INTERP,#USRDM ;Cause the c-m to be started. It will go into wait.
BR 5$ ;Done
4$: CMP #CMDRA,CMTYPE(R0)
BEQ 5$ ;If duration type then done
FETCH CMBITS(R0) ;Get force sensing bits for c-m
5$: POP <R2,R0> ;Retrieve env pointer & count
DEC R0 ; & make as many cmons as we were told to
BLE 6$
JMP 1$
6$: RTS PC ;Done
; CMENBL, CMDSBL, CMDEST, CMTRIG, CMSKED, CMWAIT, CMUNCR
CMENBL: ;Interpeter routine
; One argument, a level-offset pair for the c-m to enable.
FETCH R0 ;R0 ← level-offset
JSR PC,GETARG ;R0 ← pointer into environment
MOV 2(R0),R0 ;R0 ← pointer to c-m control block.
BEQ CMDERR ;If none, then error
CMP #CMDRA,CMTYPE(R0) ;see what type of c-m we've got
BGT 2$ ;If event or expression then skip ahead
BIT #CMENB,CMSTAT(R0) ;Already enabled?
BNE 3$ ;Then done
BIS #CMENB,CMSTAT(R0) ;Set enabled bit
BIT #CMRUN,CMSTAT(R0) ;See if currently running
BNE 3$ ; & if so we're done - it'll re-enable itself
PUSH <R4> ;Save old ISB
MOV CMISB(R0),R4 ;Get new ISB
MOV CMSTRT(R0),IPC(R4) ;Set IPC to LOC[c-m checker]
MOV RF,-(SP) ;Save RF
MOV SP,RF ;RF ← LOC[Stack]
PUSH <#1$> ;Save return address since we're not doing a JSR PC
PUSH <12(SP),12(SP)> ;Copy R3 stack limits
JMP INT1 ;Go do it - CMDUR, CMFORCE & CMSENSE return
1$: POP <R4> ;Restore old ISB
BR 2$ ;Done
2$: BIS #CMENB,CMSTAT(R0) ;Set the enable bit
EVSIG CMSEVT(R0) ;Gronk the c-m
3$: CCC ;Clear condition code
RTS PC ;Done
CMDSBL: ;Interpreter routine
; One argument, a level-offset pair for the c-m to disable.
FETCH R0 ;R0 ← level-offset
JSR PC,GETARG ;R0 ← pointer into environment
MOV 2(R0),R0 ;R0 ← pointer to c-m control block.
BEQ CMDERR ;If none, then error
JSR PC,CMDIS ;Go disable the c-m
CCC ;Clear condition code
RTS PC ;Done
CMDERR: ALERR CMNEMS
SCC ;Set condition code
RTS PC
DATA
CMNEMS::ASCIE </TRYING TO TREAT NON-EXISTENT EVENT/>
CODE
CMDEST: ;Auxillary routine
COMMENT ⊗ Called by KVAR to kill the cmon pointed at by (R2). ⊗
PUSH <R1,R2> ;Save R1 & R2
MOV (R2),R0 ;R0 ← LOC[c-m control block]
BEQ CMDERR ;If none, then error
JSR PC,CMDIS ;Make sure c-m's disabled
BIS #CMDES,CMSTAT(R0) ;Set the destroy bit
CMP #CMDRA,CMTYPE(R0) ;See what type of c-m
BGT 2$ ;If event or expression c-m then handle below
BIT #CMRUN,CMSTAT(R0) ;If running it will destroy itself
BNE 3$ ; so we're done *** should probably wait though ***
MOV CMISB(R0),R2 ;R2 ← LOC[c-m's ISB]
JSR PC,RLFREE ;Reclaim the c-m control block
MOV STKBAS(R2),R0 ;Reclaim interpreter stack
JSR PC,RLFREE
MOV ENV(R2),R0 ;Reclaim this environment
JSR PC,RLFREE
MOV PDB(R2),R0 ;Reclaim Process Descriptor Block
JSR PC,RLFREE
EVWAIT INTEVT ;Enter critical region.
MOV #ISTBLK,R0 ;The following unlinks this interpreter from the chain.
1$: MOV R0,R1
MOV NXTINT(R1),R0
CMP R0,R2 ;Have we found ours yet?
BNE 1$
MOV NXTINT(R2),NXTINT(R1) ; Yes. rechain.
EVSIG INTEVT ;Leave critical region.
MOV R2,R0 ;Reclaim Interpreter Status Block
JSR PC,RLFREE
BR 3$
2$: EVKIL CMSEVT(R0) ;Destroy the event. That ought to wake him up!
SLEEP #144 ;Wait for it to die
3$: POP <R2,R1>
RTS PC ;Done
CMDIS:
COMMENT ⊗ Routine to disable a c-m, R0 ← LOC[CMCB] ⊗
BIT #CMRUN,CMSTAT(R0) ;See if it is currrently running
BEQ 10$ ; if not then proceed
SLEEP #144 ; else wait for it to finish
BR CMDIS ; Checking every 100 milliseconds
10$: BIT #CMENB,CMSTAT(R0) ;Check if currently enabled
BEQ 3$ ; if not - done
CMP #CMDRA,CMTYPE(R0) ;See what type of c-m
BGT 2$ ;Event & expression c-m's are easy - skip ahead
BEQ 2$ ;Can't do anything with duration c-m's now
CMP #CMFRC,CMTYPE(R0)
BLT 2$ ; ditto with hardware c-m's
PUSH <R0> ;Save R0
MOV CMISB(R0),R1 ;R1 ← LOC[c-m's ISB]
MOV PDB(R1),R1 ;R1 ← LOC[c-m's PDB]
MOV CMBITS(R0),R0 ;R0 ← c-m's force sensing bits
JSR PC,@LFRCOFF ;Remove c-m from force signal list
TST R0
BEQ 1$
; ALERR CMNODS ;Complain if error (don't bother)
1$: POP <R0> ;Restore R0
2$: BIC #CMENB,CMSTAT(R0) ;Clear the enable bit
3$: RTS PC ;Done
DATA
CMNODS::ASCIE </COULDN'T DISABLE FORCE CMON/>
CODE
CMTRIG: ;Interpeter routine
COMMENT ⊗ Should be executed only from a c-m. Sets the priority to 1
and disables the checker. ⊗
MOV CMCB(R4),R0
1$: EVTST CMSEVT(R0) ;Eat all signals enabling the checker.
BCC 1$
BIC #CMENB,CMSTAT(R0) ;Clear the enable bit
BIS #CMRUN,CMSTAT(R0) ;Set the run bit
SETPRI #1 ;Set the priority to 1
TST (SP)+ ;Discard old priority
CCC ;Clear condition code
RTS PC ;Done
CMSKED: ;Interpreter routine
COMMENT ⊗ Goes to sleep a while (currently, 100 milliseconds). Upon
awakening, checks the status bits of this checker, and either
dismisses, waits, or returns. ⊗
MOV CMCB(R4),R0 ;R0 ← c-m control block
BIC #CMRUN,CMSTAT(R0) ;Clear run bit
CMP #CMEXP,CMTYPE(R0) ;See what type of c-m
BNE 1$ ;If event c-m skip ahead
SETPRI #3 ;In case the conclusion left it at 1
TST (SP)+ ;Flush old priority
FETCH -(SP) ;Waiting interval
SLEEP ;Sleep a while
1$: BIT #CMDES,CMSTAT(R0) ;Destroy bit set?
BEQ 3$ ;No
EVKIL CMSEVT(R0);Yes. Kill the triggering event.
2$: JSR PC,RLFREE ;Return the c-m control block
JMP TERMINATE ;Use the interpeter terminate routine.
3$: BIT #CMENB,CMSTAT(R0) ;Enable bit set?
BNE 4$ ;Yes.
EVWAIT CMSEVT(R0);No. Wait until signaled by the enabler
BCS 2$ ;If the enabling event died, so must we.
BR 1$ ;Else start from the awakening point.
4$: CCC ;Clear condition code
RTS PC ;Done
CMWAIT: ;Interpreter routine.
COMMENT ⊗ Used by event cmons to do the waiting. ⊗
FETCH R0 ;R0 ← level-offset pair.
CMWAI0: ;entry point for POINTY
JSR PC,GETARG ;R0 ← equivalent pointer into environment
MOV 2(R0),R1 ;R1 ← LOC[Event to wait for]
EVWAIT R1 ;Wait on that event.
MOV CMCB(R4),R0 ;R0 ← LOC[CMCB]
BCC 1$ ;Return okay? If the signaling event died, so must we.
JSR PC,RLFREE ;Return the c-m control block
JMP TERMINATE ;Use the interpeter terminate routine.
1$: BIT #CMENB,CMSTAT(R0) ;Still enabled?
BNE 2$ ;Yes. May exit.
EVSIG R1 ;Oops, we were disabled! Resignal the event.
MOV CMSTRT(R0),(R4) ;And try again. Reset IPC to start of cmon.
2$: CCC ;Clear condition code
RTS PC ;Done
CMUNCR: ;Interpreter routine.
COMMENT ⊗ Used in body of c-m. Starts uncritical section. ⊗
SETPRI #1 ;Set the priority to 1
TST (SP)+ ;Flush old priority
CCC ;Clear condition code
RTS PC ;Done
; CMDONE, CMDUR, CMFORCE, CMSENSE, COMPLY, CMPOFF, VMKFRC, TFRCST
CMDONE: ;Interpreter routine
COMMENT ⊗ Ends duration, force sensing & hardware monitor c-m's. Checks if
c-m was re-enabled while running and if so it will interpret the c-m's checker
(and so doing the c-m will be re-queued). Then it dismisses. ⊗
MOV CMCB(R4),R0 ;Get c-m control block
BIC #CMRUN,CMSTAT(R0) ;Clear run bit
BIT #CMDEST,CMSTAT(R0) ;Destroy ourself
BEQ 1$
JSR PC,RLFREE ;Yup - reclaim CMCB
JMP TERMINATE ;Use interpreter terminate routine
1$: BIT #CMENB,CMSTAT(R0) ;See if we were re-enabled
BEQ 2$ ;Nope - go away
MOV CMSTRT(R0),IPC(R4) ;Reset IPC to LOC[c-m's checker]
MOV RF,-(SP) ;Save RF
MOV SP,RF ;RF ← LOC[Stack]
JSR PC,INTERP ;Re-queue it
2$: MOV PDB(R4),R0 ;R0 ← LOC[c-m's PDB]
MOV R3,PDBR3(R0) ;Make sure stack is okay
MOV PDBPC(R0),PDBR2(R0) ;Save new PC(if any) in R2 since DISMIS kills it
DISMIS ;Bye-bye
JMP (R2) ;If return here use R2 to get where we should be
CMDUR: ;Interpreter routine
COMMENT ⊗ Schedules c-m body to be executed in time seconds. (The time is
on the stack.) Then returns control using RF. ⊗
LDF @(R3)+,AC0 ;Get time to wait in seconds
MULF THOUS,AC0 ;Convert it to milliseconds
STCFI AC0,R0 ; & make it integer
SCHEDU PDB(R4),#1$,#USRDM,R0 ;Schedule the c-m body to start later
MOV RF,SP ;Restore stack
MOV -2(SP),RF ;RF ← old PC
RTS RF ;Just return
1$: MOV CMCB(R4),R0 ;R0 ← LOC[c-m's control block]
BIT #CMENB,CMSTAT(R0) ;See if we're still enabled
BNE 2$
DISMIS ;If not then go away
2$: JMP CMGO ;Set flags & go interpret the c-m's body
VMKFRC: ;Interpreter routine
COMMENT ⊗ Takes force vector (on R3 stack) and makes it into a frame with the x-axis
along the force vector. Always followed by a call to TFRCST which calls SETC. ⊗
JSR PC,UNITV ;Make it a unit vector
JSR PC,NOCMP ;Don't compact for a while
MOV (R3)+,R0 ;R0 ← LOC[unit vector]
LDF (R0)+,AC0 ;Get X
LDF (R0)+,AC1 ;Get Y
LDF (R0)+,AC2 ;Get Z
STF AC0,AC4 ;Copy X
STF AC1,AC5 ;Copy Y
JSR PC,GETTRN ;R0 ← -(R3) ← LOC[new trans]
STF AC0,(R0)+
STF AC1,(R0)+ ;Fill in 1st column with unit vector
STF AC2,(R0)+
MULF AC0,AC0 ;X↑2
MULF AC1,AC1 ;Y↑2
ADDF AC1,AC0 ;X↑2 + Y↑2
CFCC ;Check if X = Y = 0
BNE 1$ ; & if not skip ahead
CLRF (R0)+
STF AC2,(R0)+ ;Next column is (0 Z 0)
NEGF AC2
CLRF (R0)+
STF AC2,(R0) ;Last column is (-Z 0 0)
BR 2$ ;Jump ahead
1$: JSR PC,@LSQRTF ;AC0 ← SQRT(X↑2 + Y↑2)
LDF AC4,AC3 ;Get X
LDF AC5,AC1 ;Get Y
NEGF AC1 ;Negate Y
DIVF AC0,AC1 ;a = -Y / SQRT(X↑2 + Y↑2)
DIVF AC0,AC3 ;b = X / SQRT(X↑2 + Y↑2)
STF AC1,(R0)+
STF AC3,(R0)+ ;Fill in 2nd column with (a b 0)
CLRF (R0)+
STF AC2,AC0 ;Copy Z
MULF AC1,AC2 ;aZ
MULF AC3,AC0 ;bZ
NEGF AC0 ;-bZ
MULF AC4,AC3 ;bX
MULF AC5,AC1 ;aY
SUBF AC1,AC3 ;bX - aY
STF AC0,(R0)+
STF AC2,(R0)+ ;Fill in 3rd column with(-bZ,aZ,bX-aY)
STF AC3,(R0)+ ; it's the cross product of the other 2 columns
2$: JSR PC,YESCMP ;OK to compact again
CCC
RTS PC ;Done - return
TFRCST: ;Interpreter routine
COMMENT ⊗ Gets force frame off of the R3 stack, arm & co-ordinate system bits follow
via the IPC. Calls SETC. ⊗
FETCH R0 ;Get bits for SETC: arm & c-oord system (hand/table)
MOV (R3)+,R1 ;R1 ← LOC[force coordinate matrix]
JSR PC,@LSETC ;Initialize the force system
TST R0
BEQ 1$
ALERR CMNSET ;Complain if any problems
1$: CCC
RTS PC ;Done - return
CMFORCE: ;Interpreter routine
COMMENT ⊗ Gets force value (scalar on R3 stack) and queues c-m on force signal list.
Then returns control using RF. ⊗
LDF @(R3)+,AC0 ;Get the force threshold value
MOV PDB(R4),R1 ;R1 ← LOC[c-m's PDB]
MOV CMCB(R4),R2 ;R2 ← LOC[c-m's control block]
MOV CMBITS(R2),R0 ;R0 ← c-m's force bits
MOV #CMGO,R2 ;R2 ← when triggered start below
JSR PC,@LFRCSIG ;Put the c-m in the force signal list
TST R0
BEQ 1$
ALERR CMNFRC ;Complain if any problems
1$: MOV RF,SP ;Restore stack
MOV -2(SP),RF ;RF ← old PC
RTS RF ;Just return
CMGO: MOV PDB(R4),R0 ;R0 ← LOC[c-m's PDB]
MOV USKMAX(R0),SP ;Reset stack pointer
MOV CMCB(R4),R0 ;R0 ← LOC[c-m's control block]
BIT #CMENB,CMSTAT(R0) ;Check that we are still enabled
BNE 2$ ; Yup - skip ahead
JMP CMDONE ; Nope - so go away
2$: BIS #CMRUN,CMSTAT(R0) ;Set the run bit
BIC #CMENB,CMSTAT(R0) ;Clear the enable bit
JMP INTERP ;Go interpret the c-m's body
COMPLY: ;Interpreter routine
COMMENT ⊗ Gets magnitude of force to apply (scalar on R3 stack) and the control bits via
(the arm and force component to apply) follow via the IPC. ⊗
FETCH R0 ;Get bits for COMPLY
LDF @(R3)+,AC0 ;Get the force value
JSR PC,@LBISON ;Set up the force to apply
TST R0
BEQ 1$
ALERR CMNCMP ;Complain if any problems
1$: CCC
RTS PC ;Done - return
CMPOFF: ;Interpreter routine
ALERR NOCMPF ;Complain - CMPOFF hasn't been written yet
CCC
RTS PC
CMSENSE: ALERR CMNOSE ;Aren't any of these guys yet
MOV RF,SP ;Restore stack
MOV -2(SP),RF ;RF ← old PC
RTS RF ;Just return
DATA
CMNSET::ASCIE </COULDN'T INITIALIZE FORCE SYSTEM/>
CMNFRC::ASCIE </COULDN'T QUEUE FORCE CMON/>
CMNCMP::ASCIE </COULDN'T SET UP FORCE COMPLIANCE/>
NOCMPF::ASCIE </CAN'T TURN OFF COMPLIANCE YET/>
CMNOSE::ASCIE </HARDWARE MONITORING ISN'T READY YET/>
CODE
;Events: SIGNAL, WAITE, PAUSE
COMMENT ⊗ Events can be created (at the beginnings of blocks is the
usual place), signaled, awaited (in the middle of a block) and
destroyed (at the end of a block). Each event is a variable, that
is, it is refered to by a level-offset pair. However, its place in
the environment does not point to a graph node, since there is no
such thing as attachment to an event. The event itself is stored in
the environment. The garbage collector marking phase had better
understand this. ⊗
SIGNAL: ;Interpreter routine. Signal the event of the level-offset pair.
FETCH R0 ;R0 ← level-offset pair.
SIGNL0:: ; entry point for POINTY
JSR PC,GETARG ;R0 ← equivalent pointer into environment
EVSIG 2(R0) ;Signal that event.
CCC ;Clear condition code.
RTS PC ;Done
WAITE: ;Interpreter routine. Wait on the event of the level-offset pair.
FETCH R0 ;R0 ← level-offset pair.
WAITE0: ; entry point for POINTY
JSR PC,GETARG ;R0 ← equivalent pointer into environment
EVWAIT 2(R0) ;Wait on that event.
BCC 1$ ;Return OK?
JMP TERMINATE ;The event was destroyed. I guess we should depart cleanly.
1$: JSR PC,NOTICE ;Assume the world has gone awry.
CCC ;Clear condition code.
RTS PC ;Done
PAUSE: ;Interpreter routine
COMMENT ⊗ Pause n seconds, where n is on the stack. ⊗
LDF @(R3)+,AC0 ;AC0 ← wait time
MULF THOUS,AC0 ;AC0 ← time, in milliseconds
1$: CMPF MAXWT,AC0 ;Check that we don't try to sleep for more than 30 sec
CFCC ; or the kernel won't like us
BPL 2$ ;Skip ahead if less than max allowable
SUBF MAXWT,AC0 ;Update remaining time to pause
SLEEP #30000. ;Sleep for half a minute
BR 1$ ; & wait the rest of the time
2$: STCFI AC0,R0 ;R0 ← time in milliseconds
BEQ 3$ ;Don't bother if time = 0
SLEEP R0 ;The pause that refreshes
3$: CCC ;Clear Condition code
RTS PC ;Done
DATA
THOUS: .FLT2 1000.0
MAXWT: .FLT2 30000.0
CODE
;Input routines: PROMPT, QUERY, SCALRD
PROMPT: ;Interpreter routine
EVWAIT CSLEVT ;Grab the console
1$: MOV #3$,R0 ;Say we want a "P" to proceed
JSR PC,TYPSTR
JSR PC,INCHR ;R1 ← reply char
MOV R1,R0
JSR PC,TYPCHR ;Echo it
BIC #40,R1 ;Make it upper case.
CMP #'P,R1 ;A valid response?
BEQ 2$ ; yup - proceed
BR 1$ ;Go and ask again
2$: MOV #CRLFX,R0 ;Type a crlf
JSR PC,TYPSTR
EVSIG CSLEVT ;Release the console
JSR PC,NOTICE ;Since arm may have been moved
CCC
RTS PC
DATA
3$: .BYTE 15, 12 ;crlf
.ASCIZ /Type P to proceed: /
CODE
QUERY: ;Interpreter routine
EVWAIT CSLEVT ;Grab the console
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar]
1$: MOV #5$,R0 ;Say we want a boolean
JSR PC,TYPSTR
JSR PC,INCHR ;R1 ← reply char
MOV R1,R0
JSR PC,TYPCHR ;Echo it
BIC #40,R1 ;Make it upper case.
CMP #'Y,R1 ;A yes response?
BEQ 2$ ; yup - put true on stack
CMP #'N,R1 ;A no response?
BEQ 3$ ; yup - all done
BR 1$ ;Go and ask again
2$: MOV ONE,@(R3)
3$: MOV #CRLFX,R0 ;Type a crlf
JSR PC,TYPSTR
EVSIG CSLEVT ;Release the console
JSR PC,NOTICE ;Since arm may have been moved
CCC
RTS PC
DATA
5$: .BYTE 15, 12 ;crlf
.ASCIZ /Type Y or N: /
CODE
SCALRD: ;Interpreter routine
EVWAIT CSLEVT ;Grab the console
JSR PC,GETSCA ;R0 ← -(R3) ← LOC[new scalar]
1$: MOV #2$,R0 ;Say we want a scalar
JSR PC,TYPSTR
MOV #INBUF,R0 ;Read a new line.
JSR PC,INSTR
MOV #INBUF,R0
JSR PC,RELSCN ;AC0 ← number typed in
TST R1 ;Got anything?
BNE 1$ ; nope - try again
STF AC0,@(R3) ;Put number in desired place.
EVSIG CSLEVT ;Release the console
JSR PC,NOTICE ;Since arm may have been moved
CCC
RTS PC
DATA
2$: .BYTE 15, 12 ;crlf
.ASCIZ /SCALAR, PLEASE: /
CODE
;Output routines: PRINT, VALPRN, VARPRN, TACKVAL, TYPVAL, CVFX
PRINT: ;Interpreter routine
FETCH R0 ;R0 ← Address of string
PRINT0: EVWAIT CSLEVT ;label used by POINTY
JSR PC,TYPSTR ;Type it out
EVSIG CSLEVT
CCC ;Clear condition code
RTS PC ;Done
VARPRN:
COMMENT ⊗ Interpreter routine. Prints the graph node pointed to by
the level-offset of the argument. ⊗
JSR PC,GTVAL ;Let GTVAL put value on stack
JMP VALPRN ;And let VALPRN take it from there.
VALPRN:
COMMENT ⊗ Interpreter routine. Prints the value the top of the stack
and pops it. ⊗
MOV (R3)+,R0 ;R0 ← LOC[value cell]
JSR PC,TYPVAL ;Go print it.
CCC ;Clear condition codes
RTS PC ;And return
TYPVAL:
COMMENT ⊗ R0 points to a value cell. Prints it according to its
type. Requires the floating package. ⊗
PUSH <R2,R3,#TYPSTR> ;Save R2 & R3 & address of the placing routine.
EVWAIT CSLEVT
JSR PC,TYPVL
EVSIG CSLEVT
TYPVRT: TST (SP)+ ;Get rid of the address of typing routine.
POP <R3,R2> ;Restore R3 & R2
RTS PC
;R0 = LOC[value cell], R1 = LOC[string] in some cases.
;R2, R3 are available for use.
TYPVL: MOV R0,R2 ;R2 ← LOC[value cell]
MOV #CRLFX,R0 ;CRLF
JSR PC,@2(SP)
CMPB #SCLID,TAGID(R2) ;A scalar?
BEQ 1$
CMPB #VCTID,TAGID(R2) ;A vector?
BEQ 4$
CMPB #TRNID,TAGID(R2) ;A trans?
BEQ 5$
1$: MOV #SNAME,R0
JSR PC,@2(SP) ;"SCALAR "
MOV #OUTBUF,R0
2$: LDF (R2),AC0
JSR PC,CVFX
MOV #OUTBUF,R0
JSR PC,@2(SP)
3$: MOV #CRLFX,R0 ;CRLF
JSR PC,@2(SP)
RTS PC ;Done
4$: MOV #VNAME,R0
JSR PC,@2(SP) ;"VECTOR "
MOV #OUTBUF,R0
LDF (R2)+,AC0
JSR PC,CVFX
LDF (R2)+,AC0
JSR PC,CVFX
BR 2$ ;Bum code for last field.
5$: MOV #TNAME,R0
JSR PC,@2(SP) ;"TRANS "
PUSH <R3> ;Save R3
MOV #3,R3 ;R3 ← Number of rows
6$: MOV #CRLFX,R0
JSR PC,@4(SP)
MOV #OUTBUF,R0
LDF (R2),AC0
JSR PC,CVFX
LDF 14(R2),AC0
JSR PC,CVFX
LDF 30(R2),AC0
JSR PC,CVFX
LDF 44(R2),AC0
JSR PC,CVFX
MOV #OUTBUF,R0
JSR PC,@4(SP)
ADD #4,R2 ;Next row
SOB R3,6$
MOV #CRLFX,R0
JSR PC,@4(SP)
MOV #OUTBUF,R0
MOV #3,R3 ;Now do the 0 0 0 1 row
7$: CLRF AC0
JSR PC,CVFX
SOB R3,7$
LDF ONE,AC0
JSR PC,CVFX
MOV #OUTBUF,R0
JSR PC,@4(SP)
POP <R3> ;Restore R3
BR 3$ ;Go to the exit stage
CVFX: ;Version of CVF that saves R1.
PUSH <R1>
JSR PC,CVF
POP <R1>
RTS PC
DATA
SNAME:: .ASCIZ /SCALAR /
VNAME:: .ASCIZ /VECTOR /
TNAME:: .ASCIZ /TRANS /
CODE
; BREAK, NOOP, TOPAL
BREAK: ;Interpreter routine
MOV #BRKMES,R0
JSR PC,TYPSTR
BPT ;Cause a DDT break
CCC ;Clear condition code
RTS PC ;Done
DATA
BRKMES:: ASCIE </
PROGRAM BREAK/>
CODE
TOPAL: ;Interpreter routine
COMMENT ⊗ Escape to PAL. JSRs to the pseudo code. That code
should return via:
MOV PC,R0
RTS PC
⊗
JSR PC,@IPC(R4) ;Fly
ADD #2,R0 ;R0 ← Proper new IPC
MOV R0,IPC(R4) ;Hope R4, R3 still OK!
RTS PC ;Done.
;Initialization ops: PROG, ENDP, FIXIT
PROG:
COMMENT ⊗ Zeros the value & calc fields for the variables in the system
environment initializing it & makes the main interpreter environment. ⊗
JSR PC,NEWENV ;Create the main environment
MOV R0,ENV(R4) ;Store away pointer to the environment
MOV #1,LEV(R4) ;Establish the starting lexical level
MOV #SYSENV,SLINK(R0) ;Set up the pointer to SYSENV
MOV #SYSENV+4,R1 ;R1 ← first entry in system environment
1$: BIT #HDRTYP,(R1)+ ;Check access mechanism - only header or direct
BEQ 3$ ;Handle direct accesses below
MOV (R1)+,R0 ;R0 ← LOC[header]
BIT #FTYPE,TYPE(R0) ;See if device
BEQ 2$ ;Don't zap value or invmrk fields for devices
MOV #1,INVMRK(R0) ;Now invalid
CLR VAL(R0) ;Zero old value
2$: CLR CALCS(R0) ;Kill any old calcs
BR 4$
3$: CLR (R1)+ ;Zero old value
4$: CMP #SYSEND,R1 ;Any more to init?
BHI 1$ ;Go do them
CCC ;Clear condition code
RTS PC ;Done
ENDP:
COMMENT ⊗ Releases main interpreter environment. ⊗
JMP TERMINATE ;Done with the interpreter
FIXIT:
COMMENT ⊗ This should only have to be called from DDT. Unwedges the
servos. ⊗
MOV #34,R0 ;
JSR PC,GTFREE ;Get a device block
MOV R0,-(SP) ;
MOV R0,R1 ;
JSR PC,@LINTARM ;Initialize all servos
TST R0 ;All well?
BEQ 1$ ;Yes
MOV R0,-(SP) ;No
MOV #FIXM,R0 ;Complain.
JSR PC,TYPSTR ; without getting back into DDT prematurely
; MOV (SP)+,R0 ;
; JSR PC,TYPOCT ;
mov (sp)+,r0 ; print corresponding error message
mov armsg(r0),r0
jsr pc,typstr
1$: MOV (SP)+,R0 ;
JSR PC,RLFREE ;Reclaim the device block
RTS PC ;
DATA
FIXM:: ASCIE </
CAN'T INITIALIZE ARM./>
ARMSG: arm1
arm2
arm3
arm4
arm5
arm6
arm7
arm10
arm11
arm12
arm13
arm14
arm15
arm1: ascie </
Could not attach to requested joint(s)/>
arm2: ascie </
Incorrect number of joints requested to be driven/>
arm3: ascie </
Wipers could not be read within their operating range/>
arm4: ascie </
Arm solution does not exist/>
arm5: ascie </
Unknown touch sensor requested/>
arm6: ascie </
No more free slots in touch sensor event list/>
arm7: ascie </
Arm interface power supply turnd off/>
arm10: ascie </
Reference power supply out of range/>
arm11: ascie </
Zero velocity tachometer reading out of range/>
arm12: ascie </
Attempted to switch arms while force servoing/>
arm13: ascie </
No more free slots in force sensor event list/>
arm14: ascie </
Need all 6 arm joints in order to do force sensing/compliance/>
arm15: ascie </
Can't force servo motion without polynomial/>
CODE
;BUGS
COMMENT ⊗
No way to kill enabled event cmons. Need to add a kernel call that removes
a given pdb from a given event wait list.
⊗